From dba26eff918ee395e676bf59d43accd1feb19994 Mon Sep 17 00:00:00 2001 From: Jens Geisler Date: Wed, 20 Feb 2013 14:56:47 +0100 Subject: [PATCH 01/73] implemented a mixture of Sonny's MATLAB and my previous grbl planner ontop of the edge planner examples run byte for byte identical old and new version --- planner.c | 271 ++++++++++++++++++++------------------------ sim/avr/interrupt.c | 1 + sim/avr/interrupt.h | 1 + stepper.c | 6 + stepper.h | 2 + 5 files changed, 131 insertions(+), 150 deletions(-) diff --git a/planner.c b/planner.c index baaba6c..2234ef5 100644 --- a/planner.c +++ b/planner.c @@ -22,14 +22,17 @@ /* The ring buffer implementation gleaned from the wiring_serial library by David A. Mellis. */ +#include #include #include +#include #include "planner.h" #include "nuts_bolts.h" #include "stepper.h" #include "settings.h" #include "config.h" #include "protocol.h" +#include "motion_control.h" #define SOME_LARGE_VALUE 1.0E+38 // Used by rapids and acceleration maximization calculations. Just needs // to be larger than any feasible (mm/min)^2 or mm/sec^2 value. @@ -38,6 +41,7 @@ static block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion ins static volatile uint8_t block_buffer_head; // Index of the next block to be pushed static volatile uint8_t block_buffer_tail; // Index of the block to process now static uint8_t next_buffer_head; // Index of the next buffer head +static uint8_t planned_block_tail; // Index of the latest block that is optimally planned // static *block_t block_buffer_planned; // Define planner variables @@ -94,10 +98,10 @@ static uint8_t prev_block_index(uint8_t block_index) the new initial rate and n_steps until deceleration are computed, since the stepper algorithm already handles acceleration and cruising and just needs to know when to start decelerating. */ -static void calculate_trapezoid_for_block(block_t *block, float entry_speed_sqr, float exit_speed_sqr) +static uint8_t calculate_trapezoid_for_block(block_t *block, uint8_t idx, float entry_speed_sqr, float exit_speed_sqr) { // Compute new initial rate for stepper algorithm - block->initial_rate = ceil(sqrt(entry_speed_sqr)*(RANADE_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + uint32_t initial_rate = ceil(sqrt(entry_speed_sqr)*(RANADE_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) // TODO: Compute new nominal rate if a feedrate override occurs. // block->nominal_rate = ceil(feed_rate*(RANADE_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) @@ -112,19 +116,36 @@ static void calculate_trapezoid_for_block(block_t *block, float entry_speed_sqr, // Check if this is a pure acceleration block by a intersection distance less than zero. Also // prevents signed and unsigned integer conversion errors. - if (intersect_distance <= 0) { - block->decelerate_after = 0; - } else { + uint32_t decelerate_after= 0; + if (intersect_distance > 0) { // Determine deceleration distance (in steps) from nominal speed to exit speed for a trapezoidal profile. // Value is never negative. Nominal speed is always greater than or equal to the exit speed. // Computes: steps_decelerate = steps/mm * ( (v_nominal^2 - v_exit^2)/(2*acceleration) ) - block->decelerate_after = ceil(steps_per_mm_div_2_acc * (block->nominal_speed_sqr - exit_speed_sqr)); + decelerate_after = ceil(steps_per_mm_div_2_acc * (block->nominal_speed_sqr - exit_speed_sqr)); // The lesser of the two triangle and trapezoid distances always defines the velocity profile. - if (block->decelerate_after > intersect_distance) { block->decelerate_after = intersect_distance; } + if (decelerate_after > intersect_distance) { decelerate_after = intersect_distance; } // Finally, check if this is a pure deceleration block. - if (block->decelerate_after > block->step_event_count) { block->decelerate_after = block->step_event_count; } + if (decelerate_after > block->step_event_count) { decelerate_after = block->step_event_count; } + } + + // safe block adjustment + cli(); + uint8_t block_buffer_tail_hold= block_buffer_tail; // store to avoid reading volatile twice + uint8_t block_buffer_head_hold= block_buffer_head; // store to avoid reading volatile twice + uint8_t idx_inside_queue; + // is the current block inside the queue? if not: the stepper overtook us + if(block_buffer_head_hold>=block_buffer_tail_hold) idx_inside_queue= idx>=block_buffer_tail_hold && idx<=block_buffer_head_hold; + else idx_inside_queue= idx<=block_buffer_head_hold || idx>=block_buffer_tail_hold; + if(idx_inside_queue && (idx!=block_buffer_tail_hold || idx==block_buffer_head_hold || !st_is_decelerating())) { + block->decelerate_after= decelerate_after; + block->initial_rate= initial_rate; + sei(); + return(true); + } else { + sei(); + return(false); // this block is currently being processed by the stepper and it already finished accelerating or the stepper is already finished with this block: we can no longer change anything here } } @@ -183,162 +204,104 @@ static void calculate_trapezoid_for_block(block_t *block, float entry_speed_sqr, can execute faster than new blocks can be added, and the planner buffer will then starve and empty, leading to weird hiccup-like jerky motions. */ -static void planner_recalculate() +static uint8_t planner_recalculate() { - -// float entry_speed_sqr; -// uint8_t block_index = block_buffer_head; -// block_t *previous = NULL; -// block_t *current = NULL; -// block_t *next; -// while (block_index != block_buffer_tail) { -// block_index = prev_block_index( block_index ); -// next = current; -// current = previous; -// previous = &block_buffer[block_index]; -// -// if (next && current) { -// if (next != block_buffer_planned) { -// if (previous == block_buffer_tail) { block_buffer_planned = next; } -// else { -// -// if (current->entry_speed_sqr != current->max_entry_speed_sqr) { -// current->recalculate_flag = true; // Almost always changes. So force recalculate. -// entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters; -// if (entry_speed_sqr < current->max_entry_speed_sqr) { -// current->entry_speed_sqr = entry_speed_sqr; -// } else { -// current->entry_speed_sqr = current->max_entry_speed_sqr; -// } -// } else { -// block_buffer_planned = current; -// } -// } -// } else { -// break; -// } -// } -// } -// -// block_index = block_buffer_planned; -// next = &block_buffer[block_index]; -// current = prev_block_index(block_index); -// while (block_index != block_buffer_head) { -// -// // If the current 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 exit speed accordingly. Entry -// // speeds have already been reset, maximized, and reverse planned by reverse planner. -// if (current->entry_speed_sqr < next->entry_speed_sqr) { -// // Compute block exit speed based on the current block speed and distance -// // Computes: v_exit^2 = v_entry^2 + 2*acceleration*distance -// entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters; -// -// // If it's less than the stored value, update the exit speed and set recalculate flag. -// if (entry_speed_sqr < next->entry_speed_sqr) { -// next->entry_speed_sqr = entry_speed_sqr; -// next->recalculate_flag = true; -// } -// } -// -// // Recalculate if current block entry or exit junction speed has changed. -// if (current->recalculate_flag || next->recalculate_flag) { -// // NOTE: Entry and exit factors always > 0 by all previous logic operations. -// calculate_trapezoid_for_block(current, current->entry_speed_sqr, next->entry_speed_sqr); -// current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed -// } -// -// current = next; -// next = &block_buffer[block_index]; -// block_index = next_block_index( block_index ); -// } -// -// // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated. -// calculate_trapezoid_for_block(next, next->entry_speed_sqr, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED); -// next->recalculate_flag = false; + uint8_t current_block_idx= block_buffer_head; + block_t *curr_block = &block_buffer[current_block_idx]; + uint8_t plan_unchanged= 1; - // TODO: No over-write protection exists for the executing block. For most cases this has proven to be ok, but - // for feed-rate overrides, something like this is essential. Place a request here to the stepper driver to - // find out where in the planner buffer is the a safe place to begin re-planning from. + if(current_block_idx!=block_buffer_tail) { // we cannot do anything to only one block + float max_entry_speed_sqr; + float next_entry_speed_sqr= 0.0; + // loop backwards to possibly postpone deceleration + while(current_block_idx!=planned_block_tail) { // the second block is the one where we start the forward loop + if(current_block_idx==block_buffer_tail) { + planned_block_tail= current_block_idx; + break; + } -// if (block_buffer_head != block_buffer_tail) { - float entry_speed_sqr; + // TODO: Determine maximum entry speed at junction for feedrate overrides, since they can alter + // the planner nominal speeds at any time. This calc could be done in the override handler, but + // this could require an additional variable to be stored to differentiate the programmed nominal + // speeds, max junction speed, and override speeds/scalar. - // Perform reverse planner pass. Skip the head(end) block since it is already initialized, and skip the - // tail(first) block to prevent over-writing of the initial entry speed. - uint8_t block_index = prev_block_index( block_buffer_head ); // Assume buffer is not empty. - block_t *current = &block_buffer[block_index]; // Head block-1 = Newly appended block - block_t *next; - if (block_index != block_buffer_tail) { block_index = prev_block_index( block_index ); } - while (block_index != block_buffer_tail) { - next = current; - current = &block_buffer[block_index]; - - // TODO: Determine maximum entry speed at junction for feedrate overrides, since they can alter - // the planner nominal speeds at any time. This calc could be done in the override handler, but - // this could require an additional variable to be stored to differentiate the programmed nominal - // speeds, max junction speed, and override speeds/scalar. - - // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising. - // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and - // check for maximum allowable speed reductions to ensure maximum possible planned speed. - if (current->entry_speed_sqr != current->max_entry_speed_sqr) { + // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising. + // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and + // check for maximum allowable speed reductions to ensure maximum possible planned speed. + if (curr_block->entry_speed_sqr != curr_block->max_entry_speed_sqr) { + // default if next_entry_speed_sqr > curr_block->max_entry_speed_sqr || max_entry_speed_sqr > curr_block->max_entry_speed_sqr + curr_block->entry_speed_sqr = curr_block->max_entry_speed_sqr; - current->entry_speed_sqr = current->max_entry_speed_sqr; - current->recalculate_flag = true; // Almost always changes. So force recalculate. - - if (next->entry_speed_sqr < current->max_entry_speed_sqr) { - // Computes: v_entry^2 = v_exit^2 + 2*acceleration*distance - entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters; - if (entry_speed_sqr < current->max_entry_speed_sqr) { - current->entry_speed_sqr = entry_speed_sqr; + if (next_entry_speed_sqr < curr_block->max_entry_speed_sqr) { + // Computes: v_entry^2 = v_exit^2 + 2*acceleration*distance + max_entry_speed_sqr = next_entry_speed_sqr + 2*curr_block->acceleration*curr_block->millimeters; + if (max_entry_speed_sqr < curr_block->max_entry_speed_sqr) { + curr_block->entry_speed_sqr = max_entry_speed_sqr; + } } - } - } - block_index = prev_block_index( block_index ); - } + } + next_entry_speed_sqr= curr_block->entry_speed_sqr; - // Perform forward planner pass. Begins junction speed adjustments after tail(first) block. - // Also recalculate trapezoids, block by block, as the forward pass completes the plan. - block_index = next_block_index(block_buffer_tail); - next = &block_buffer[block_buffer_tail]; // Places tail(first) block into current - while (block_index != block_buffer_head) { - current = next; - next = &block_buffer[block_index]; + current_block_idx= prev_block_index( current_block_idx ); + curr_block= &block_buffer[current_block_idx]; + } + + // loop forward, adjust exit speed to not exceed max accelleration + block_t *next_block; + uint8_t next_block_idx; + float max_exit_speed_sqr; + while(current_block_idx!=block_buffer_head) { + next_block_idx= next_block_index(current_block_idx); + next_block = &block_buffer[next_block_idx]; // If the current 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 exit speed accordingly. Entry // speeds have already been reset, maximized, and reverse planned by reverse planner. - if (current->entry_speed_sqr < next->entry_speed_sqr) { + if (curr_block->entry_speed_sqr < next_block->entry_speed_sqr) { // Compute block exit speed based on the current block speed and distance // Computes: v_exit^2 = v_entry^2 + 2*acceleration*distance - entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters; + max_exit_speed_sqr = curr_block->entry_speed_sqr + 2*curr_block->acceleration*curr_block->millimeters; - // If it's less than the stored value, update the exit speed and set recalculate flag. - if (entry_speed_sqr < next->entry_speed_sqr) { - next->entry_speed_sqr = entry_speed_sqr; - next->recalculate_flag = true; - } + } else { + max_exit_speed_sqr= SOME_LARGE_VALUE; } - // Recalculate if current block entry or exit junction speed has changed. - if (current->recalculate_flag || next->recalculate_flag) { - // NOTE: Entry and exit factors always > 0 by all previous logic operations. - calculate_trapezoid_for_block(current, current->entry_speed_sqr, next->entry_speed_sqr); - current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed + // adjust max_exit_speed_sqr in case this is a deceleration block or max accel cannot be reached + if(max_exit_speed_sqr>next_block->entry_speed_sqr) { + max_exit_speed_sqr= next_block->entry_speed_sqr; + } else { + // this block has reached max acceleration, it is optimal + planned_block_tail= next_block_idx; } - block_index = next_block_index( block_index ); + if(calculate_trapezoid_for_block(curr_block, current_block_idx, curr_block->entry_speed_sqr, max_exit_speed_sqr)) { + next_block->entry_speed_sqr= max_exit_speed_sqr; + plan_unchanged= 0; + } else if(!plan_unchanged) { // we started to modify the plan an then got overtaken by the stepper executing the plan: this is bad + return(0); + } + + // Check if the next block entry speed is at max_entry_speed. If so, move the planned pointer, since + // this entry speed cannot be improved anymore and all prior blocks have been completed and optimally planned. + if(next_block->entry_speed_sqr>=next_block->max_entry_speed_sqr) { + planned_block_tail= next_block_idx; + } + + current_block_idx= next_block_idx; + curr_block= next_block; + } } - // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated. - calculate_trapezoid_for_block(next, next->entry_speed_sqr, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED); - next->recalculate_flag = false; -// } + if(!calculate_trapezoid_for_block(curr_block, current_block_idx, curr_block->entry_speed_sqr, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED)) { + // this can only happen to the first block in the queue? so we dont need to clear or stop anything + return(0); + } else + return(1); } void plan_init() { - block_buffer_tail = block_buffer_head; + block_buffer_tail = block_buffer_head= planned_block_tail; next_buffer_head = next_block_index(block_buffer_head); // block_buffer_planned = block_buffer_head; memset(&pl, 0, sizeof(pl)); // Clear planner struct @@ -409,7 +372,7 @@ void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert // Compute path vector in terms of absolute step target and current positions float delta_mm[N_AXIS]; - delta_mm[X_AXIS] = x-pl.last_x; + delta_mm[X_AXIS] = x-pl.last_x; // what difference would it make to use block->steps_x/settings.steps_per_mm[X_AXIS]; instead? delta_mm[Y_AXIS] = y-pl.last_y; delta_mm[Z_AXIS] = z-pl.last_z; block->millimeters = sqrt(delta_mm[X_AXIS]*delta_mm[X_AXIS] + delta_mm[Y_AXIS]*delta_mm[Y_AXIS] + @@ -448,13 +411,14 @@ void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert block->nominal_rate = ceil(feed_rate*(RANADE_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) // Compute the acceleration and distance traveled per step event for the stepper algorithm. + // TODO: obsolete? block->rate_delta = ceil(block->acceleration* ((RANADE_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) block->d_next = ceil((block->millimeters*RANADE_MULTIPLIER)/block->step_event_count); // (mult*mm/step) // Compute direction bits. Bit enabled always means direction is negative. block->direction_bits = 0; - if (unit_vec[X_AXIS] < 0) { block->direction_bits |= (1<direction_bits |= (1<steps_x if (unit_vec[Y_AXIS] < 0) { block->direction_bits |= (1<direction_bits |= (1<max_entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_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) && (pl.previous_nominal_speed_sqr > 0.0)) { // 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. @@ -496,13 +460,11 @@ void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert } } - // Initialize block entry speed. Compute block entry velocity backwards from user-defined MINIMUM_PLANNER_SPEED. - // TODO: This could be moved to the planner recalculate function. - block->entry_speed_sqr = min( block->max_entry_speed_sqr, - MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED + 2*block->acceleration*block->millimeters); + // Initialize block entry speed + block->entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED; // Set new block to be recalculated for conversion to stepper data. - block->recalculate_flag = true; + block->recalculate_flag = true; // TODO: obsolete? // Update previous path unit_vector and nominal speed (squared) memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // pl.previous_unit_vec[] = unit_vec[] @@ -514,11 +476,20 @@ void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert pl.last_y = y; pl.last_z = z; + if(!planner_recalculate()) { + // TODO: make alarm informative + if (sys.state != STATE_ALARM) { + if (bit_isfalse(sys.execute,EXEC_ALARM)) { + mc_reset(); // Initiate system kill. + sys.execute |= EXEC_CRIT_EVENT; // Indicate hard limit critical event + } + } + } + // Update buffer head and next buffer head indices + // Mind that updating block_buffer_head after the planner changes the planner logic a bit block_buffer_head = next_buffer_head; next_buffer_head = next_block_index(block_buffer_head); - - planner_recalculate(); } // Reset the planner position vectors. Called by the system abort/initialization routine. diff --git a/sim/avr/interrupt.c b/sim/avr/interrupt.c index f47e11d..10b5225 100644 --- a/sim/avr/interrupt.c +++ b/sim/avr/interrupt.c @@ -40,3 +40,4 @@ uint16_t pcmsk0; uint16_t pcicr; void sei() {}; +void cli() {}; \ No newline at end of file diff --git a/sim/avr/interrupt.h b/sim/avr/interrupt.h index 5b17cc3..1cb1eb5 100644 --- a/sim/avr/interrupt.h +++ b/sim/avr/interrupt.h @@ -49,6 +49,7 @@ extern uint16_t pcicr; // enable interrupts does nothing in the simulation environment void sei(); +void cli(); // dummy macros for interrupt related registers #define TIMSK0 timsk0 diff --git a/stepper.c b/stepper.c index f8d6c35..77213ea 100644 --- a/stepper.c +++ b/stepper.c @@ -62,6 +62,7 @@ static uint8_t out_bits; // The next stepping-bits to be output // this blocking variable is no longer needed. Only here for safety reasons. static volatile uint8_t busy; // True when "Stepper Driver Interrupt" is being serviced. Used to avoid retriggering that handler. + // __________________________ // /| |\ _________________ ^ // / | | \ /| |\ | @@ -380,3 +381,8 @@ void st_cycle_reinitialize() sys.state = STATE_IDLE; } } + +uint8_t st_is_decelerating() { + return st.ramp_type == DECEL_RAMP; +} + diff --git a/stepper.h b/stepper.h index 0cb4189..5cfe6ba 100644 --- a/stepper.h +++ b/stepper.h @@ -45,4 +45,6 @@ void st_cycle_reinitialize(); // Initiates a feed hold of the running program void st_feed_hold(); +uint8_t st_is_decelerating(); + #endif From 87864cce19c36096f27eb5431c2bc2f4deba24f5 Mon Sep 17 00:00:00 2001 From: Jens Geisler Date: Wed, 20 Feb 2013 15:04:26 +0100 Subject: [PATCH 02/73] added counter for planner steps --- planner.c | 4 ++++ planner.h | 2 ++ sim/simulator.c | 3 ++- 3 files changed, 8 insertions(+), 1 deletion(-) diff --git a/planner.c b/planner.c index 2234ef5..43fa937 100644 --- a/planner.c +++ b/planner.c @@ -34,6 +34,8 @@ #include "protocol.h" #include "motion_control.h" +uint32_t planner_steps_counter; + #define SOME_LARGE_VALUE 1.0E+38 // Used by rapids and acceleration maximization calculations. Just needs // to be larger than any feasible (mm/min)^2 or mm/sec^2 value. @@ -210,6 +212,7 @@ static uint8_t planner_recalculate() block_t *curr_block = &block_buffer[current_block_idx]; uint8_t plan_unchanged= 1; + planner_steps_counter= 0; if(current_block_idx!=block_buffer_tail) { // we cannot do anything to only one block float max_entry_speed_sqr; float next_entry_speed_sqr= 0.0; @@ -219,6 +222,7 @@ static uint8_t planner_recalculate() planned_block_tail= current_block_idx; break; } + planner_steps_counter++; // TODO: Determine maximum entry speed at junction for feedrate overrides, since they can alter // the planner nominal speeds at any time. This calc could be done in the override handler, but diff --git a/planner.h b/planner.h index a16c8c9..7d065cd 100644 --- a/planner.h +++ b/planner.h @@ -22,6 +22,8 @@ #ifndef planner_h #define planner_h +extern uint32_t planner_steps_counter; + // The number of linear motions that can be in the plan at any give time #ifndef BLOCK_BUFFER_SIZE #define BLOCK_BUFFER_SIZE 18 diff --git a/sim/simulator.c b/sim/simulator.c index 73c1405..bbdb624 100644 --- a/sim/simulator.c +++ b/sim/simulator.c @@ -157,7 +157,8 @@ void printBlock() { else block_position[2]+= b->steps_z; fprintf(block_out_file,"%d, ", block_position[2]); - fprintf(block_out_file,"%f", b->entry_speed_sqr); + fprintf(block_out_file,"%f, ", b->entry_speed_sqr); + fprintf(block_out_file,"%d", planner_steps_counter); fprintf(block_out_file,"\n"); last_block= b; From 40d8b8bf66a8872b41320b7af9056436bbaea980 Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Wed, 20 Feb 2013 09:23:12 -0700 Subject: [PATCH 03/73] Update README.md --- README.md | 23 ++--------------------- 1 file changed, 2 insertions(+), 21 deletions(-) diff --git a/README.md b/README.md index 88bd4fb..8945d1b 100644 --- a/README.md +++ b/README.md @@ -1,23 +1,4 @@ -#Grbl - An embedded g-code interpreter and motion-controller for the Arduino/AVR328 microcontroller +#Grbl ------------ -Grbl is a no-compromise, high performance, low cost alternative to parallel-port-based motion control for CNC milling. It will run on a vanilla Arduino (Duemillanove/Uno) as long as it sports an Atmega 328. - -The controller is written in highly optimized C utilizing every clever feature of the AVR-chips to achieve precise timing and asynchronous operation. It is able to m aintain more than 30kHz of stable, jitter free control pulses. - -It accepts standards-compliant G-code and has been tested with the output of several CAM tools with no problems. Arcs, circles and helical motion are fully supported, as well as, other basic functional g-code commands. Functions and variables are not currently supported, but may be included in future releases in a form of a pre-processor. - -Grbl includes full acceleration management with look ahead. That means the controller will look up to 18 motions into the future and plan its velocities ahead to deliver smooth acceleration and jerk-free cornering. - -##Changelog for v0.9 from v0.8 - - **ALPHA status: Under heavy development.** - - New stepper algorithm: Based on the Pramod Ranade inverse time algorithm, but modified to ensure steps are executed exactly. This algorithm performs a constant timer tick and has a hard limit of 30kHz maximum step frequency. It is also highly tuneable and should be very easy to port to other microcontroller architectures. - - Planner optimizations: Multiple changes to increase planner execution speed and removed redundant variables. - - Acceleration independence: Each axes may be defined with different acceleration parameters and Grbl will automagically calculate the maximum acceleration through a path depending on the direction traveled. This is very useful for machine that have very different axes properties, like the ShapeOko z-axis. - - Maximum velocity independence: As with acceleration, the maximum velocity of individual axes may be defined. All seek/rapids motions will move at these maximum rates, but never exceed any one axes. So, when two or more axes move, the limiting axis will move at its maximum rate, while the other axes are scaled down. - - Significantly improved arc performance: Arcs are now defined in terms of chordal tolerance, rather than segment length. Chordal tolerance will automatically scale all arc line segments depending on arc radius, such that the error does not exceed the tolerance value (default: 0.005 mm.) So, for larger radii arcs, Grbl can move faster through them, because the segments are always longer and the planner has more distance to plan with. - - New Grbl SIMULATOR by @jgeisler: A completely independent wrapper of the Grbl main source code that may be compiled as an executable on a computer. No Arduino required. Simply simulates the responses of Grbl as if it was on an Arduino. May be used for many things: checking out how Grbl works, pre-process moves for GUI graphics, debugging of new features, etc. Much left to do, but potentially very powerful, as the dummy AVR variables can be written to output anything you need. - - Feedrate overrides: In the works, but planner has begun to be re-factored for this feature. - - Jogging controls: Methodology needs to be to figured out first. Last item on the agenda. - -_The project was initially inspired by the Arduino GCode Interpreter by Mike Ellery_ +This branch serves only as a developmental platform for working on new ideas that may eventually be installed into Grbl's edge branch. Please do not use as there is no guarantee this code base is up-to-date or working. From ea09ddba99bf739043161316bb3fa04c671113a0 Mon Sep 17 00:00:00 2001 From: Jens Geisler Date: Fri, 22 Feb 2013 16:36:27 +0100 Subject: [PATCH 04/73] changed atomic access for updating the acceleration profile the stepper interrupt is only halted when necessary and for the shortest time possible (8% cycle time) --- Makefile | 2 +- planner.c | 63 ++++++++++++++++++++++++++++++++++--------------- planner.h | 2 -- sim/simulator.c | 3 +-- stepper.h | 1 + 5 files changed, 47 insertions(+), 24 deletions(-) diff --git a/Makefile b/Makefile index f0e7f41..707de73 100644 --- a/Makefile +++ b/Makefile @@ -42,7 +42,7 @@ FUSES = -U hfuse:w:0xd2:m -U lfuse:w:0xff:m # Tune the lines below only if you know what you are doing: AVRDUDE = avrdude $(PROGRAMMER) -p $(DEVICE) -B 10 -F -COMPILE = avr-gcc -Wall -Os -DF_CPU=$(CLOCK) -mmcu=$(DEVICE) -I. -ffunction-sections +COMPILE = avr-gcc -Wall -Os -DF_CPU=$(CLOCK) -mmcu=$(DEVICE) -I. -ffunction-sections --std=c99 # symbolic targets: all: grbl.hex diff --git a/planner.c b/planner.c index 43fa937..19a7558 100644 --- a/planner.c +++ b/planner.c @@ -23,6 +23,7 @@ /* The ring buffer implementation gleaned from the wiring_serial library by David A. Mellis. */ #include +#include #include #include #include @@ -34,8 +35,6 @@ #include "protocol.h" #include "motion_control.h" -uint32_t planner_steps_counter; - #define SOME_LARGE_VALUE 1.0E+38 // Used by rapids and acceleration maximization calculations. Just needs // to be larger than any feasible (mm/min)^2 or mm/sec^2 value. @@ -103,7 +102,8 @@ static uint8_t prev_block_index(uint8_t block_index) static uint8_t calculate_trapezoid_for_block(block_t *block, uint8_t idx, float entry_speed_sqr, float exit_speed_sqr) { // Compute new initial rate for stepper algorithm - uint32_t initial_rate = ceil(sqrt(entry_speed_sqr)*(RANADE_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + // volatile is necessary so that the optimizer doesn't move the calculation in the ATOMIC_BLOCK + volatile uint32_t initial_rate = ceil(sqrt(entry_speed_sqr)*(RANADE_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) // TODO: Compute new nominal rate if a feedrate override occurs. // block->nominal_rate = ceil(feed_rate*(RANADE_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) @@ -132,23 +132,50 @@ static uint8_t calculate_trapezoid_for_block(block_t *block, uint8_t idx, float if (decelerate_after > block->step_event_count) { decelerate_after = block->step_event_count; } } - // safe block adjustment - cli(); - uint8_t block_buffer_tail_hold= block_buffer_tail; // store to avoid reading volatile twice - uint8_t block_buffer_head_hold= block_buffer_head; // store to avoid reading volatile twice - uint8_t idx_inside_queue; - // is the current block inside the queue? if not: the stepper overtook us - if(block_buffer_head_hold>=block_buffer_tail_hold) idx_inside_queue= idx>=block_buffer_tail_hold && idx<=block_buffer_head_hold; - else idx_inside_queue= idx<=block_buffer_head_hold || idx>=block_buffer_tail_hold; - if(idx_inside_queue && (idx!=block_buffer_tail_hold || idx==block_buffer_head_hold || !st_is_decelerating())) { + uint8_t block_buffer_tail_hold= block_buffer_tail; // store to avoid rereading volatile + // check if we got overtaken by the stepper + if(idx==prev_block_index(block_buffer_tail_hold)) { + return false; + } + + // check where the stepper is currently working relative to the block we want to update + uint8_t block_buffer_tail_next= next_block_index(block_buffer_tail_hold); + if(idx==block_buffer_tail_hold || idx==block_buffer_tail_next) { + // we are close to were the stepper is working, so we need to block it for a short time + // to safely adjust the block + + // I counted the cycles in this block from the assembler code + // It's 42 cycles worst case including the call to st_is_decelerating + // @ 16MHz this is 2.6250e-06 seconds, 30kHz cycle duration is 3.3333e-05 seconds + // -> this block will delay the stepper timer by max 8% + // given that this occurs not very often, it should be ok + // but test will have to show + + // ATOMIC_BLOCK only works with compiler parameter --std=c99 + ATOMIC_BLOCK(ATOMIC_FORCEON) { + // reload block_buffer_tail in case it changed + uint8_t block_buffer_tail_hold2= block_buffer_tail; + if(idx!=block_buffer_tail_hold2) { + if(block_buffer_tail_hold2==block_buffer_tail_next) + return false; // the stepper didn't overtook in the meantime + } else { + if(st_is_decelerating()) + return false; // we want to change the currently running block and it has already started to decelerate + } + + block->decelerate_after= decelerate_after; + block->initial_rate= initial_rate; + return true; + } + } else { + // let's assume the stepper did not complete two blocks since we loaded block_buffer_tail to block_buffer_tail_hold + // so the block we want to change is not currently being run by the stepper and it's safe to touch it without precautions block->decelerate_after= decelerate_after; block->initial_rate= initial_rate; - sei(); - return(true); - } else { - sei(); - return(false); // this block is currently being processed by the stepper and it already finished accelerating or the stepper is already finished with this block: we can no longer change anything here + return true; } + + return false; } @@ -212,7 +239,6 @@ static uint8_t planner_recalculate() block_t *curr_block = &block_buffer[current_block_idx]; uint8_t plan_unchanged= 1; - planner_steps_counter= 0; if(current_block_idx!=block_buffer_tail) { // we cannot do anything to only one block float max_entry_speed_sqr; float next_entry_speed_sqr= 0.0; @@ -222,7 +248,6 @@ static uint8_t planner_recalculate() planned_block_tail= current_block_idx; break; } - planner_steps_counter++; // TODO: Determine maximum entry speed at junction for feedrate overrides, since they can alter // the planner nominal speeds at any time. This calc could be done in the override handler, but diff --git a/planner.h b/planner.h index 7d065cd..a16c8c9 100644 --- a/planner.h +++ b/planner.h @@ -22,8 +22,6 @@ #ifndef planner_h #define planner_h -extern uint32_t planner_steps_counter; - // The number of linear motions that can be in the plan at any give time #ifndef BLOCK_BUFFER_SIZE #define BLOCK_BUFFER_SIZE 18 diff --git a/sim/simulator.c b/sim/simulator.c index bbdb624..73c1405 100644 --- a/sim/simulator.c +++ b/sim/simulator.c @@ -157,8 +157,7 @@ void printBlock() { else block_position[2]+= b->steps_z; fprintf(block_out_file,"%d, ", block_position[2]); - fprintf(block_out_file,"%f, ", b->entry_speed_sqr); - fprintf(block_out_file,"%d", planner_steps_counter); + fprintf(block_out_file,"%f", b->entry_speed_sqr); fprintf(block_out_file,"\n"); last_block= b; diff --git a/stepper.h b/stepper.h index 5cfe6ba..2419a0b 100644 --- a/stepper.h +++ b/stepper.h @@ -45,6 +45,7 @@ void st_cycle_reinitialize(); // Initiates a feed hold of the running program void st_feed_hold(); +// Accessor function to query the acceleration state of the stepper uint8_t st_is_decelerating(); #endif From a85e1b80f7ccf8b58fa44c235cfa5d4a8bb47b9a Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Fri, 22 Feb 2013 13:23:13 -0700 Subject: [PATCH 05/73] Push additional updates from @jgeisler0303 --- planner.c | 12 ++++++------ planner.h | 1 + stepper.c | 1 + 3 files changed, 8 insertions(+), 6 deletions(-) diff --git a/planner.c b/planner.c index 19a7558..dd231e2 100644 --- a/planner.c +++ b/planner.c @@ -259,17 +259,17 @@ static uint8_t planner_recalculate() // check for maximum allowable speed reductions to ensure maximum possible planned speed. if (curr_block->entry_speed_sqr != curr_block->max_entry_speed_sqr) { // default if next_entry_speed_sqr > curr_block->max_entry_speed_sqr || max_entry_speed_sqr > curr_block->max_entry_speed_sqr - curr_block->entry_speed_sqr = curr_block->max_entry_speed_sqr; + curr_block->new_entry_speed_sqr = curr_block->max_entry_speed_sqr; if (next_entry_speed_sqr < curr_block->max_entry_speed_sqr) { // Computes: v_entry^2 = v_exit^2 + 2*acceleration*distance max_entry_speed_sqr = next_entry_speed_sqr + 2*curr_block->acceleration*curr_block->millimeters; if (max_entry_speed_sqr < curr_block->max_entry_speed_sqr) { - curr_block->entry_speed_sqr = max_entry_speed_sqr; + curr_block->new_entry_speed_sqr = max_entry_speed_sqr; } } } - next_entry_speed_sqr= curr_block->entry_speed_sqr; + next_entry_speed_sqr= curr_block->new_entry_speed_sqr; current_block_idx= prev_block_index( current_block_idx ); curr_block= &block_buffer[current_block_idx]; @@ -286,7 +286,7 @@ static uint8_t planner_recalculate() // If the current 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 exit speed accordingly. Entry // speeds have already been reset, maximized, and reverse planned by reverse planner. - if (curr_block->entry_speed_sqr < next_block->entry_speed_sqr) { + if (curr_block->entry_speed_sqr < next_block->new_entry_speed_sqr) { // Compute block exit speed based on the current block speed and distance // Computes: v_exit^2 = v_entry^2 + 2*acceleration*distance max_exit_speed_sqr = curr_block->entry_speed_sqr + 2*curr_block->acceleration*curr_block->millimeters; @@ -296,8 +296,8 @@ static uint8_t planner_recalculate() } // adjust max_exit_speed_sqr in case this is a deceleration block or max accel cannot be reached - if(max_exit_speed_sqr>next_block->entry_speed_sqr) { - max_exit_speed_sqr= next_block->entry_speed_sqr; + if(max_exit_speed_sqr>next_block->new_entry_speed_sqr) { + max_exit_speed_sqr= next_block->new_entry_speed_sqr; } else { // this block has reached max acceleration, it is optimal planned_block_tail= next_block_idx; diff --git a/planner.h b/planner.h index a16c8c9..693344d 100644 --- a/planner.h +++ b/planner.h @@ -40,6 +40,7 @@ typedef struct { float nominal_speed_sqr; // The nominal speed for this block in mm/min float entry_speed_sqr; // Entry speed at previous-current block junction in mm/min float max_entry_speed_sqr; // Maximum allowable junction entry speed in mm/min + float new_entry_speed_sqr; // Temporary entry speed used by the planner float millimeters; // The total travel of this block in mm float acceleration; uint8_t recalculate_flag; // Planner flag to recalculate trapezoids on entry junction diff --git a/stepper.c b/stepper.c index 77213ea..8c066af 100644 --- a/stepper.c +++ b/stepper.c @@ -120,6 +120,7 @@ void st_go_idle() STEPPERS_DISABLE_PORT |= (1< Date: Fri, 22 Feb 2013 14:11:14 -0700 Subject: [PATCH 06/73] Added some prelimary notes to new changes. --- planner.c | 28 ++++++++++++++++------------ planner.h | 1 - 2 files changed, 16 insertions(+), 13 deletions(-) diff --git a/planner.c b/planner.c index dd231e2..8d8d04c 100644 --- a/planner.c +++ b/planner.c @@ -105,7 +105,7 @@ static uint8_t calculate_trapezoid_for_block(block_t *block, uint8_t idx, float // volatile is necessary so that the optimizer doesn't move the calculation in the ATOMIC_BLOCK volatile uint32_t initial_rate = ceil(sqrt(entry_speed_sqr)*(RANADE_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) - // TODO: Compute new nominal rate if a feedrate override occurs. + // TODO: Compute new nominal rate if a feedrate override occurs. Could be performed by simple scalar. // block->nominal_rate = ceil(feed_rate*(RANADE_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) // Compute efficiency variable for following calculations. Removes a float divide and multiply. @@ -400,8 +400,9 @@ void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert if (block->step_event_count == 0) { return; }; // Compute path vector in terms of absolute step target and current positions + // NOTE: Operates by arithmetic rather than expensive division. float delta_mm[N_AXIS]; - delta_mm[X_AXIS] = x-pl.last_x; // what difference would it make to use block->steps_x/settings.steps_per_mm[X_AXIS]; instead? + delta_mm[X_AXIS] = x-pl.last_x; delta_mm[Y_AXIS] = y-pl.last_y; delta_mm[Z_AXIS] = z-pl.last_z; block->millimeters = sqrt(delta_mm[X_AXIS]*delta_mm[X_AXIS] + delta_mm[Y_AXIS]*delta_mm[Y_AXIS] + @@ -435,19 +436,23 @@ void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert } } - // Compute nominal speed and rates + // Compute nominal speed block->nominal_speed_sqr = feed_rate*feed_rate; // (mm/min)^2. Always > 0 - block->nominal_rate = ceil(feed_rate*(RANADE_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) - // Compute the acceleration and distance traveled per step event for the stepper algorithm. - // TODO: obsolete? + // Pre-calculate stepper algorithm values: Acceleration rate, distance traveled per step event, and nominal rate. + // TODO: Obsolete? Sort of. This pre-calculates this value so the stepper algorithm doesn't have to upon loading. + // The multiply and ceil() may take too many cycles but removing it would save (BUFFER_SIZE-1)*4 bytes of memory. + block->nominal_rate = ceil(feed_rate*(RANADE_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) block->rate_delta = ceil(block->acceleration* ((RANADE_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) block->d_next = ceil((block->millimeters*RANADE_MULTIPLIER)/block->step_event_count); // (mult*mm/step) // Compute direction bits. Bit enabled always means direction is negative. + // TODO: Check if this can be combined with steps_x calcs to speed up. Not sure though since + // this only has to perform a negative check on already existing values. I think I've measured + // the speed difference. This should be optimal in speed and flash space, I believe. block->direction_bits = 0; - if (unit_vec[X_AXIS] < 0) { block->direction_bits |= (1<steps_x + if (unit_vec[X_AXIS] < 0) { block->direction_bits |= (1<direction_bits |= (1<direction_bits |= (1<entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED; - // Set new block to be recalculated for conversion to stepper data. - block->recalculate_flag = true; // TODO: obsolete? - // Update previous path unit_vector and nominal speed (squared) memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // pl.previous_unit_vec[] = unit_vec[] pl.previous_nominal_speed_sqr = block->nominal_speed_sqr; @@ -516,7 +520,8 @@ void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert } // Update buffer head and next buffer head indices - // Mind that updating block_buffer_head after the planner changes the planner logic a bit + // NOTE: Mind that updating block_buffer_head after the planner changes the planner logic a bit + // TODO: Check if this is better to place after recalculate or before in terms of buffer executing. block_buffer_head = next_buffer_head; next_buffer_head = next_block_index(block_buffer_head); } @@ -547,6 +552,5 @@ void plan_cycle_reinitialize(int32_t step_events_remaining) // Re-plan from a complete stop. Reset planner entry speeds and flags. block->entry_speed_sqr = 0.0; block->max_entry_speed_sqr = 0.0; - block->recalculate_flag = true; planner_recalculate(); } diff --git a/planner.h b/planner.h index 693344d..ff83016 100644 --- a/planner.h +++ b/planner.h @@ -43,7 +43,6 @@ typedef struct { float new_entry_speed_sqr; // Temporary entry speed used by the planner float millimeters; // The total travel of this block in mm float acceleration; - uint8_t recalculate_flag; // Planner flag to recalculate trapezoids on entry junction // Settings for the trapezoid generator uint32_t initial_rate; // The step rate at start of block From b75e95c88019051bddb93bb6192492fab4a650ae Mon Sep 17 00:00:00 2001 From: bungao Date: Tue, 26 Feb 2013 13:55:12 +1100 Subject: [PATCH 07/73] integrating soft limits --- limits.c | 1 + motion_control.c | 15 ++++++++++++++- protocol.c | 6 ++---- report.c | 8 +++++++- report.h | 1 + settings.c | 7 +++++++ settings.h | 3 ++- 7 files changed, 34 insertions(+), 7 deletions(-) diff --git a/limits.c b/limits.c index 1da9c54..91180ce 100644 --- a/limits.c +++ b/limits.c @@ -73,6 +73,7 @@ ISR(LIMIT_INT_vect) if (sys.state != STATE_ALARM) { if (bit_isfalse(sys.execute,EXEC_ALARM)) { mc_reset(); // Initiate system kill. + report_alarm_message(ALARM_HARD_LIMIT); sys.execute |= EXEC_CRIT_EVENT; // Indicate hard limit critical event } } diff --git a/motion_control.c b/motion_control.c index ca23c87..6305bb4 100644 --- a/motion_control.c +++ b/motion_control.c @@ -35,6 +35,7 @@ #include "planner.h" #include "limits.h" #include "protocol.h" +#include "report.h" // Execute linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second // unless invert_feed_rate is true. Then the feed_rate means that the motion should be completed in @@ -49,9 +50,21 @@ // backlash segment(s). void mc_line(float x, float y, float z, float feed_rate, uint8_t invert_feed_rate) { - // TODO: Perform soft limit check here. Just check if the target x,y,z values are outside the + // TO TEST: Perform soft limit check here. Just check if the target x,y,z values are outside the // work envelope. Should be straightforward and efficient. By placing it here, rather than in // the g-code parser, it directly picks up motions from everywhere in Grbl. + if (bit_istrue(settings.flags,BITFLAG_SOFT_LIMIT_ENABLE)) { + if( (x> settings.mm_soft_limit[X_AXIS])||(y>settings.mm_soft_limit[Y_AXIS])||(z>settings.mm_soft_limit[Z_AXIS])) { + if (sys.state != STATE_ALARM) { + if (bit_isfalse(sys.execute,EXEC_ALARM)) { + mc_reset(); // Initiate system kill. + report_alarm_message(ALARM_SOFT_LIMIT); + sys.state = STATE_ALARM; + sys.execute |= EXEC_CRIT_EVENT; // Indicate hard limit critical event + } + } + } + } // If in check gcode mode, prevent motion by blocking planner. if (sys.state == STATE_CHECK_MODE) { return; } diff --git a/protocol.c b/protocol.c index c97770a..fd64c3d 100644 --- a/protocol.c +++ b/protocol.c @@ -104,10 +104,8 @@ void protocol_execute_runtime() // loop until system reset/abort. if (rt_exec & (EXEC_ALARM | EXEC_CRIT_EVENT)) { sys.state = STATE_ALARM; // Set system alarm state - - // Critical event. Only hard limit qualifies. Update this as new critical events surface. - if (rt_exec & EXEC_CRIT_EVENT) { - report_alarm_message(ALARM_HARD_LIMIT); + + if (rt_exec & EXEC_CRIT_EVENT) { report_feedback_message(MESSAGE_CRITICAL_EVENT); bit_false(sys.execute,EXEC_RESET); // Disable any existing reset do { diff --git a/report.c b/report.c index 6b810ad..0895c6a 100644 --- a/report.c +++ b/report.c @@ -88,6 +88,8 @@ void report_alarm_message(int8_t alarm_code) printPgmString(PSTR("Hard limit")); break; case ALARM_ABORT_CYCLE: printPgmString(PSTR("Abort during cycle")); break; + case ALARM_SOFT_LIMIT: + printPgmString(PSTR("Soft Limit")); break; } printPgmString(PSTR(". MPos?\r\n")); delay_ms(500); // Force delay to ensure message clears serial write buffer. @@ -172,7 +174,11 @@ void report_grbl_settings() { printPgmString(PSTR(" (homing feed, mm/min)\r\n$23=")); printFloat(settings.homing_seek_rate); printPgmString(PSTR(" (homing seek, mm/min)\r\n$24=")); printInteger(settings.homing_debounce_delay); printPgmString(PSTR(" (homing debounce, msec)\r\n$25=")); printFloat(settings.homing_pulloff); - printPgmString(PSTR(" (homing pull-off, mm)\r\n")); + printPgmString(PSTR(" (homing pull-off, mm)\r\n$26=")); printFloat(settings.mm_soft_limit[X_AXIS]); + printPgmString(PSTR(" (x, max travel)\r\n$27=")); printFloat(settings.mm_soft_limit[Y_AXIS]); + printPgmString(PSTR(" (y, max travel)\r\n$28=")); printFloat(settings.mm_soft_limit[Z_AXIS]); + printPgmString(PSTR(" (z, max travel)\r\n$29=")); printInteger(bit_istrue(settings.flags,BITFLAG_SOFT_LIMIT_ENABLE)); + printPgmString(PSTR(" (soft limits enabled, bool)\r\n")); } diff --git a/report.h b/report.h index 8f1555c..cd3cb2b 100644 --- a/report.h +++ b/report.h @@ -39,6 +39,7 @@ // Define Grbl alarm codes. Less than zero to distinguish alarm error from status error. #define ALARM_HARD_LIMIT -1 #define ALARM_ABORT_CYCLE -2 +#define ALARM_SOFT_LIMIT -3 // Define Grbl feedback message codes. #define MESSAGE_CRITICAL_EVENT 1 diff --git a/settings.c b/settings.c index 2f75e6e..798d62c 100644 --- a/settings.c +++ b/settings.c @@ -200,6 +200,13 @@ uint8_t settings_store_global_setting(int parameter, float value) { case 23: settings.homing_seek_rate = value; break; case 24: settings.homing_debounce_delay = round(value); break; case 25: settings.homing_pulloff = value; break; + case 26: case 27: case 28: + if (value <= 0.0) { return(STATUS_SETTING_VALUE_NEG); } + settings.mm_soft_limit[parameter-26] = value; break; + case 29: + if (value) { settings.flags |= BITFLAG_SOFT_LIMIT_ENABLE; } + else { settings.flags &= ~BITFLAG_SOFT_LIMIT_ENABLE; } + break; default: return(STATUS_INVALID_STATEMENT); } diff --git a/settings.h b/settings.h index 221c4fb..b614fa3 100644 --- a/settings.h +++ b/settings.h @@ -37,6 +37,7 @@ #define BITFLAG_INVERT_ST_ENABLE bit(2) #define BITFLAG_HARD_LIMIT_ENABLE bit(3) #define BITFLAG_HOMING_ENABLE bit(4) +#define BITFLAG_SOFT_LIMIT_ENABLE bit(5) // Define EEPROM memory address location values for Grbl settings and parameters // NOTE: The Atmega328p has 1KB EEPROM. The upper half is reserved for parameters and @@ -74,7 +75,7 @@ typedef struct { uint8_t stepper_idle_lock_time; // If max value 255, steppers do not disable. uint8_t decimal_places; float max_velocity[N_AXIS]; -// float mm_soft_limit[N_AXIS]; + float mm_soft_limit[N_AXIS]; // uint8_t status_report_mask; // Mask to indicate desired report data. } settings_t; extern settings_t settings; From a0f430d18816a466a83fa523b7971522959e5353 Mon Sep 17 00:00:00 2001 From: Jens Geisler Date: Tue, 26 Feb 2013 08:40:43 +0100 Subject: [PATCH 08/73] bugfix: uninitiallized curr_block->new_entry_speed_sqr lead to step loss in some cases --- planner.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/planner.c b/planner.c index 8d8d04c..50a0ea3 100644 --- a/planner.c +++ b/planner.c @@ -257,7 +257,7 @@ static uint8_t planner_recalculate() // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising. // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and // check for maximum allowable speed reductions to ensure maximum possible planned speed. - if (curr_block->entry_speed_sqr != curr_block->max_entry_speed_sqr) { + if (curr_block->entry_speed_sqr >= curr_block->max_entry_speed_sqr) { // default if next_entry_speed_sqr > curr_block->max_entry_speed_sqr || max_entry_speed_sqr > curr_block->max_entry_speed_sqr curr_block->new_entry_speed_sqr = curr_block->max_entry_speed_sqr; @@ -268,6 +268,8 @@ static uint8_t planner_recalculate() curr_block->new_entry_speed_sqr = max_entry_speed_sqr; } } + } else { + curr_block->new_entry_speed_sqr = curr_block->entry_speed_sqr; } next_entry_speed_sqr= curr_block->new_entry_speed_sqr; From 74b2af3c2f16d83f3e2d3371a23c5cc6b32cdfea Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Fri, 1 Mar 2013 09:55:10 -0700 Subject: [PATCH 09/73] Minor changes and added notes to soft limits routines. - Changed up mc_line to accept an array rather than individual x,y,z coordinates. Makes some of the position data handling more effective, especially for a 4th-axis later on. - Changed up some soft limits variable names. --- gcode.c | 9 ++++---- motion_control.c | 56 +++++++++++++++++++++++++++--------------------- motion_control.h | 2 +- report.c | 10 ++++----- settings.c | 6 +++--- settings.h | 6 +++--- 6 files changed, 47 insertions(+), 42 deletions(-) diff --git a/gcode.c b/gcode.c index d67ace0..8e7c28e 100644 --- a/gcode.c +++ b/gcode.c @@ -321,14 +321,14 @@ uint8_t gc_execute_line(char *line) target[i] = gc.position[i]; } } - mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], -1.0, false); + mc_line(target, -1.0, false); } // Retreive G28/30 go-home position data (in machine coordinates) from EEPROM float coord_data[N_AXIS]; uint8_t home_select = SETTING_INDEX_G28; if (non_modal_action == NON_MODAL_GO_HOME_1) { home_select = SETTING_INDEX_G30; } if (!settings_read_coord_data(home_select,coord_data)) { return(STATUS_SETTING_READ_FAIL); } - mc_line(coord_data[X_AXIS], coord_data[Y_AXIS], coord_data[Z_AXIS], -1.0, false); + mc_line(coord_data, -1.0, false); memcpy(gc.position, coord_data, sizeof(coord_data)); // gc.position[] = coord_data[]; axis_words = 0; // Axis words used. Lock out from motion modes by clearing flags. break; @@ -399,7 +399,7 @@ uint8_t gc_execute_line(char *line) break; case MOTION_MODE_SEEK: if (!axis_words) { FAIL(STATUS_INVALID_STATEMENT);} - else { mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], -1.0, false); } + else { mc_line(target, -1.0, false); } break; case MOTION_MODE_LINEAR: // TODO: Inverse time requires F-word with each statement. Need to do a check. Also need @@ -407,8 +407,7 @@ uint8_t gc_execute_line(char *line) // and after an inverse time move and then check for non-zero feed rate each time. This // should be efficient and effective. if (!axis_words) { FAIL(STATUS_INVALID_STATEMENT);} - else { 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); } + else { mc_line(target, (gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode); } break; case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC: // Check if at least one of the axes of the selected plane has been specified. If in center diff --git a/motion_control.c b/motion_control.c index 6305bb4..04900a2 100644 --- a/motion_control.c +++ b/motion_control.c @@ -48,24 +48,31 @@ // However, this keeps the memory requirements lower since it doesn't have to call and hold two // plan_buffer_lines in memory. Grbl only has to retain the original line input variables during a // backlash segment(s). -void mc_line(float x, float y, float z, float feed_rate, uint8_t invert_feed_rate) +void mc_line(float *target, float feed_rate, uint8_t invert_feed_rate) { // TO TEST: Perform soft limit check here. Just check if the target x,y,z values are outside the // work envelope. Should be straightforward and efficient. By placing it here, rather than in // the g-code parser, it directly picks up motions from everywhere in Grbl. - if (bit_istrue(settings.flags,BITFLAG_SOFT_LIMIT_ENABLE)) { - if( (x> settings.mm_soft_limit[X_AXIS])||(y>settings.mm_soft_limit[Y_AXIS])||(z>settings.mm_soft_limit[Z_AXIS])) { - if (sys.state != STATE_ALARM) { - if (bit_isfalse(sys.execute,EXEC_ALARM)) { - mc_reset(); // Initiate system kill. - report_alarm_message(ALARM_SOFT_LIMIT); - sys.state = STATE_ALARM; - sys.execute |= EXEC_CRIT_EVENT; // Indicate hard limit critical event - } - } + // TODO: Eventually move the soft limit check into limits.c. + if (bit_istrue(settings.flags,BITFLAG_SOFT_LIMIT_ENABLE)) { + uint8_t i; + for (i=0; i settings.max_travel[i])) { + // TODO: Need to make this more in-line with the rest of the alarm and runtime execution handling. + // Not quite right. Also this should force Grbl to feed hold and exit, rather than stopping and alarm + // out. This would help retain machine position, but is this really required? + if (sys.state != STATE_ALARM) { + if (bit_isfalse(sys.execute,EXEC_ALARM)) { + mc_reset(); // Initiate system kill. + report_alarm_message(ALARM_SOFT_LIMIT); + sys.state = STATE_ALARM; + sys.execute |= EXEC_CRIT_EVENT; // Indicate hard limit critical event + } } + } + } } - + // If in check gcode mode, prevent motion by blocking planner. if (sys.state == STATE_CHECK_MODE) { return; } @@ -82,7 +89,7 @@ void mc_line(float x, float y, float z, float feed_rate, uint8_t invert_feed_rat protocol_execute_runtime(); // Check for any run-time commands if (sys.abort) { return; } // Bail, if system abort. } while ( plan_check_full_buffer() ); - plan_buffer_line(x, y, z, feed_rate, invert_feed_rate); + plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], feed_rate, invert_feed_rate); // If idle, indicate to the system there is now a planned block in the buffer ready to cycle // start. Otherwise ignore and continue on. @@ -96,6 +103,9 @@ void mc_line(float x, float y, float z, float feed_rate, uint8_t invert_feed_rat // when the buffer is completely full and primed; auto-starting, if there was only one g-code // command sent during manual operation; or if a system is prone to buffer starvation, auto-start // helps make sure it minimizes any dwelling/motion hiccups and keeps the cycle going. + // NOTE: Moved into main loop and plan_check_full_buffer() as a test. This forces Grbl to process + // all of the commands in the serial read buffer or until the planner buffer is full before auto + // cycle starting. Will eventually need to remove the following command. // if (sys.auto_start) { st_cycle_start(); } } @@ -204,14 +214,14 @@ void mc_arc(float *position, float *target, float *offset, uint8_t axis_0, uint8 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); + mc_line(arc_target, 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); + mc_line(target, feed_rate, invert_feed_rate); } @@ -252,22 +262,18 @@ void mc_go_home() // Pull-off axes (that have been homed) from limit switches before continuing motion. // This provides some initial clearance off the switches and should also help prevent them // from falsely tripping when hard limits are enabled. - int8_t x_dir, y_dir, z_dir; - x_dir = y_dir = z_dir = 0; + float target[N_AXIS]; + target[X_AXIS] = target[Y_AXIS] = target[Z_AXIS] = settings.homing_pulloff; if (HOMING_LOCATE_CYCLE & (1< Date: Fri, 1 Mar 2013 10:50:36 -0700 Subject: [PATCH 10/73] Bug fix to-do note on soft limit checks. Not yet completed. --- motion_control.c | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/motion_control.c b/motion_control.c index 04900a2..0834940 100644 --- a/motion_control.c +++ b/motion_control.c @@ -57,6 +57,9 @@ void mc_line(float *target, float feed_rate, uint8_t invert_feed_rate) if (bit_istrue(settings.flags,BITFLAG_SOFT_LIMIT_ENABLE)) { uint8_t i; for (i=0; i settings.max_travel[i])) { // TODO: Need to make this more in-line with the rest of the alarm and runtime execution handling. // Not quite right. Also this should force Grbl to feed hold and exit, rather than stopping and alarm @@ -265,13 +268,13 @@ void mc_go_home() float target[N_AXIS]; target[X_AXIS] = target[Y_AXIS] = target[Z_AXIS] = settings.homing_pulloff; if (HOMING_LOCATE_CYCLE & (1< Date: Fri, 5 Apr 2013 09:21:52 -0600 Subject: [PATCH 11/73] Updates to edge/dev. Line buffer increased/planner buffer decreased. Line overflow feedback. - Increased g-code parser line buffer to 70 characters (from 50) to prevent some long arc commands from getting truncated. - Decreased planner buffer from 18 to 17 blocks to free up memory for line buffer. - Added a line buffer overflow feedback error (Thanks @BHSPitMonkey!) --- config.h | 4 +- gcode.c | 7 +- planner.h | 2 +- protocol.c | 21 ++- protocol.h | 2 +- report.c | 2 + report.h | 1 + stepper.c | 6 + stepper_new.c | 424 ++++++++++++++++++++++++++++++++++++++++++++++++++ 9 files changed, 454 insertions(+), 15 deletions(-) create mode 100644 stepper_new.c diff --git a/config.h b/config.h index d1587fa..3791b31 100644 --- a/config.h +++ b/config.h @@ -210,7 +210,7 @@ // available RAM, like when re-compiling for a Mega or Sanguino. Or decrease if the Arduino // begins to crash due to the lack of available RAM or if the CPU is having trouble keeping // up with planning new incoming motions as they are executed. -// #define BLOCK_BUFFER_SIZE 18 // Uncomment to override default in planner.h. +// #define BLOCK_BUFFER_SIZE 17 // Uncomment to override default in planner.h. // Line buffer size from the serial input stream to be executed. Also, governs the size of // each of the startup blocks, as they are each stored as a string of this size. Make sure @@ -220,7 +220,7 @@ // can be too small and g-code blocks can get truncated. Officially, the g-code standards // support up to 256 characters. In future versions, this default will be increased, when // we know how much extra memory space we can re-invest into this. -// #define LINE_BUFFER_SIZE 50 // Uncomment to override default in protocol.h +// #define LINE_BUFFER_SIZE 70 // Uncomment to override default in protocol.h // Serial send and receive buffer size. The receive buffer is often used as another streaming // buffer to store incoming blocks to be processed by Grbl when its ready. Most streaming diff --git a/gcode.c b/gcode.c index b17a34a..d87ce52 100644 --- a/gcode.c +++ b/gcode.c @@ -245,7 +245,8 @@ uint8_t gc_execute_line(char *line) // If there were any errors parsing this line, we will return right away with the bad news if (gc.status_code) { return(gc.status_code); } - + uint8_t i; + /* Execute Commands: Perform by order of execution defined in NIST RS274-NGC.v3, Table 8, pg.41. */ // ([F]: Set feed rate.) @@ -290,7 +291,6 @@ uint8_t gc_execute_line(char *line) else { int_value = gc.coord_select; } // Index P0 as the active coordinate system float coord_data[N_AXIS]; if (!settings_read_coord_data(int_value,coord_data)) { return(STATUS_SETTING_READ_FAIL); } - uint8_t i; // Update axes defined only in block. Always in machine coordinates. Can change non-active system. for (i=0; i= LINE_BUFFER_SIZE-1) { - // Throw away any characters beyond the end of the line buffer + // Detect line buffer overflow. Report error and reset line buffer. + report_status_message(STATUS_OVERFLOW); + protocol_reset_line_buffer(); } else if (c >= 'a' && c <= 'z') { // Upcase lowercase line[char_counter++] = c-'a'+'A'; } else { diff --git a/protocol.h b/protocol.h index 209d19d..4d90c1c 100644 --- a/protocol.h +++ b/protocol.h @@ -30,7 +30,7 @@ // memory space we can invest into here or we re-write the g-code parser not to have his // buffer. #ifndef LINE_BUFFER_SIZE - #define LINE_BUFFER_SIZE 50 + #define LINE_BUFFER_SIZE 70 #endif // Initialize the serial protocol diff --git a/report.c b/report.c index c115dc7..8c90a17 100644 --- a/report.c +++ b/report.c @@ -76,6 +76,8 @@ void report_status_message(uint8_t status_code) printPgmString(PSTR("Alarm lock")); break; case STATUS_SOFT_LIMIT_ERROR: printPgmString(PSTR("Homing not enabled")); break; + case STATUS_OVERFLOW: + printPgmString(PSTR("Line overflow")); break; } printPgmString(PSTR("\r\n")); } diff --git a/report.h b/report.h index cd7f42d..7dcfc47 100644 --- a/report.h +++ b/report.h @@ -36,6 +36,7 @@ #define STATUS_IDLE_ERROR 11 #define STATUS_ALARM_LOCK 12 #define STATUS_SOFT_LIMIT_ERROR 13 +#define STATUS_OVERFLOW 14 // Define Grbl alarm codes. Less than zero to distinguish alarm error from status error. #define ALARM_LIMIT_ERROR -1 diff --git a/stepper.c b/stepper.c index cac20f3..acb3282 100644 --- a/stepper.c +++ b/stepper.c @@ -78,6 +78,7 @@ static volatile uint8_t busy; // True when "Stepper Driver Interrupt" is being // after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as // +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. + // Stepper state initialization. Cycle should only start if the st.cycle_start flag is // enabled. Startup init and limits call this function but shouldn't start the cycle. void st_wake_up() @@ -101,6 +102,7 @@ void st_wake_up() } } + // Stepper shutdown void st_go_idle() { @@ -297,6 +299,7 @@ ISR(TIMER2_COMPA_vect) // SPINDLE_ENABLE_PORT ^= 1<. +*/ + +#include +#include "stepper.h" +#include "config.h" +#include "settings.h" +#include "planner.h" + +// Some useful constants +#define TICKS_PER_MICROSECOND (F_CPU/1000000) +#define CRUISE_RAMP 0 +#define ACCEL_RAMP 1 +#define DECEL_RAMP 2 + +// Stepper state variable. Contains running data and trapezoid variables. +typedef struct { + // Used by the bresenham line algorithm + int32_t counter_x, // Counter variables for the bresenham line tracer + counter_y, + counter_z; + uint32_t event_count; // Total event count. Retained for feed holds. + uint32_t step_events_remaining; // Steps remaining in motion + + // Used by Pramod Ranade inverse time algorithm + int32_t delta_d; // Ranade distance traveled per interrupt tick + int32_t d_counter; // Ranade distance traveled since last step event + uint8_t ramp_count; // Acceleration interrupt tick counter. + uint8_t ramp_type; // Ramp type variable. + uint8_t execute_step; // Flags step execution for each interrupt. + +} stepper_t; +static stepper_t st; +static block_t *current_block; // A pointer to the block currently being traced + +// Used by the stepper driver interrupt +static uint8_t step_pulse_time; // Step pulse reset time after step rise +static uint8_t out_bits; // The next stepping-bits to be output + +// NOTE: If the main interrupt is guaranteed to be complete before the next interrupt, then +// this blocking variable is no longer needed. Only here for safety reasons. +static volatile uint8_t busy; // True when "Stepper Driver Interrupt" is being serviced. Used to avoid retriggering that handler. + +// __________________________ +// /| |\ _________________ ^ +// / | | \ /| |\ | +// / | | \ / | | \ s +// / | | | | | \ p +// / | | | | | \ e +// +-----+------------------------+---+--+---------------+----+ e +// | BLOCK 1 | BLOCK 2 | d +// +// time -----> +// +// The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta +// until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after +// after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as +// +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. + + +// Stepper state initialization. Cycle should only start if the st.cycle_start flag is +// enabled. Startup init and limits call this function but shouldn't start the cycle. +void st_wake_up() +{ + // Enable steppers by resetting the stepper disable port + if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { + STEPPERS_DISABLE_PORT |= (1<> 3); + // Enable stepper driver interrupt + st.execute_step = false; + TCNT0 = 0; // Clear Timer2 + TIMSK0 |= (1<d_next; + + // Load next step + out_bits = current_block->direction_bits; // Reset out_bits and reload direction bits + st.execute_step = true; + + // Execute step displacement profile by Bresenham line algorithm + st.counter_x -= current_block->steps_x; + if (st.counter_x < 0) { + out_bits |= (1<steps_y; + if (st.counter_y < 0) { + out_bits |= (1<steps_z; + if (st.counter_z < 0) { + out_bits |= (1<direction_bits ^ settings.invert_mask; + st.execute_step = true; // Set flag to set direction bits. + + // Initialize Bresenham variables + st.counter_x = (current_block->step_event_count >> 1); + st.counter_y = st.counter_x; + st.counter_z = st.counter_x; + st.event_count = current_block->step_event_count; + st.step_events_remaining = st.event_count; + + // During feed hold, do not update inverse time counter, rate, or ramp type. Keep decelerating. + if (sys.state == STATE_CYCLE) { + // Initialize Ranade variables + st.d_counter = current_block->d_next; + st.delta_d = current_block->initial_rate; + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; + + // Initialize ramp type. + if (st.step_events_remaining == current_block->decelerate_after) { st.ramp_type = DECEL_RAMP; } + else if (st.delta_d == current_block->nominal_rate) { st.ramp_type = CRUISE_RAMP; } + else { st.ramp_type = ACCEL_RAMP; } + } + + } else { + st_go_idle(); + bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end + return; // Nothing to do but exit. + } + } + + // Adjust inverse time counter for ac/de-celerations + if (st.ramp_type) { + // Tick acceleration ramp counter + st.ramp_count--; + if (st.ramp_count == 0) { + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter + if (st.ramp_type == ACCEL_RAMP) { // Adjust velocity for acceleration + st.delta_d += current_block->rate_delta; + if (st.delta_d >= current_block->nominal_rate) { // Reached cruise state. + st.ramp_type = CRUISE_RAMP; + st.delta_d = current_block->nominal_rate; // Set cruise velocity + } + } else if (st.ramp_type == DECEL_RAMP) { // Adjust velocity for deceleration + if (st.delta_d > current_block->rate_delta) { + st.delta_d -= current_block->rate_delta; + } else { + st.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow. + } + } + } + } + + + // Check for feed hold state and execute accordingly. + if (sys.state == STATE_HOLD) { + if (st.ramp_type != DECEL_RAMP) { + st.ramp_type = DECEL_RAMP; + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; + } + if (st.delta_d <= current_block->rate_delta) { + st_go_idle(); + bit_true(sys.execute,EXEC_CYCLE_STOP); + return; + } + } + + if (st.ramp_type != DECEL_RAMP) { + // Acceleration and cruise handled by ramping. Just check for deceleration. + if (st.step_events_remaining <= current_block->decelerate_after) { + st.ramp_type = DECEL_RAMP; + if (st.step_events_remaining == current_block->decelerate_after) { + if (st.delta_d == current_block->nominal_rate) { + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid + } else { + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; // Set ramp counter for triangle + } + } + } + } + } else { + + busy = false; +} + + +// The Stepper Port Reset Interrupt: Timer2 OVF interrupt handles the falling edge of the +// step pulse. This should always trigger before the next Timer0 COMPA interrupt and independently +// finish, if Timer0 is disabled after completing a move. +ISR(TIMER2_OVF_vect) +{ + STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | (settings.invert_mask & STEP_MASK); + TCCR2B = 0; // Disable timer until needed. +} + + +// Reset and clear stepper subsystem variables +void st_reset() +{ + memset(&st, 0, sizeof(st)); + current_block = NULL; + busy = false; +} + + +// Initialize and start the stepper motor subsystem +void st_init() +{ + // Configure directions of interface pins + STEPPING_DDR |= STEPPING_MASK; + STEPPING_PORT = (STEPPING_PORT & ~STEPPING_MASK) | settings.invert_mask; + STEPPERS_DISABLE_DDR |= 1< Date: Mon, 19 Aug 2013 09:24:22 -0600 Subject: [PATCH 12/73] Push old dev_2 draft to work on other things. - **NON-FUNCTIONAL** - Contains an old draft of separating the stepper driver direct access to the planner buffer. This is designed to keep the stepper and planner modules independent and prevent overwriting or other complications. In this way, feedrate override should be able to be installed as well. - A number of planner optimizations are installed too. - Not sure where the bugs are. Either in the new planner optimizations, new stepper module updates, or in both. Or it just could be that the Arduino AVR is choking with the new things it has to do. --- config.h | 9 +- gcode.c | 270 +++++----- gcode.h | 8 +- limits.c | 42 +- main.c | 5 +- motion_control.c | 33 +- nuts_bolts.c | 15 +- nuts_bolts.h | 3 +- planner.c | 577 +++++++++----------- planner.h | 49 +- planner_old.c | 476 +++++++++++++++++ planner_old.h | 83 +++ print.c | 2 +- print.h | 2 + protocol.c | 1 + report.c | 6 +- settings.c | 11 +- stepper.c | 571 ++++++++++++++------ stepper.h | 2 + stepper_new2.c | 601 +++++++++++++++++++++ stepper_new.c => stepper_new_dual_ISR.c | 31 +- stepper_new_time.c | 680 ++++++++++++++++++++++++ stepper_old.c | 387 ++++++++++++++ 23 files changed, 3160 insertions(+), 704 deletions(-) create mode 100644 planner_old.c create mode 100644 planner_old.h create mode 100644 stepper_new2.c rename stepper_new.c => stepper_new_dual_ISR.c (96%) create mode 100644 stepper_new_time.c create mode 100644 stepper_old.c diff --git a/config.h b/config.h index 3791b31..c1c450e 100644 --- a/config.h +++ b/config.h @@ -25,7 +25,7 @@ // IMPORTANT: Any changes here requires a full re-compiling of the source code to propagate them. // Default settings. Used when resetting EEPROM. Change to desired name in defaults.h -#define DEFAULTS_GENERIC +#define DEFAULTS_ZEN_TOOLWORKS_7x7 // Serial baud rate #define BAUD_RATE 9600 @@ -113,7 +113,7 @@ // interrupt of the current stepper driver algorithm theoretically up to a frequency of 35-40kHz, but // CPU overhead increases exponentially as this frequency goes up. So there will be little left for // other processes like arcs. -#define ISR_TICKS_PER_SECOND 30000L // Integer (Hz) +#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 @@ -121,7 +121,8 @@ // 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 120L +// NOTE: Ramp count variable type in stepper module may need to be updated if changed. +#define ACCELERATION_TICKS_PER_SECOND 100L // NOTE: Make sure this value is less than 256, when adjusting both dependent parameters. #define ISR_TICKS_PER_ACCELERATION_TICK (ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND) @@ -134,7 +135,7 @@ // applications, the following multiplier value will work more than well enough. If you do have // happened to weird stepper motion issues, try modifying this value by adding or subtracting a // zero and report it to the Grbl administrators. -#define RANADE_MULTIPLIER 100000000.0 +#define INV_TIME_MULTIPLIER 10000000.0 // Minimum planner junction speed. Sets the default minimum speed the planner plans for at the end // of the buffer and all stops. This should not be much greater than zero and should only be changed diff --git a/gcode.c b/gcode.c index d87ce52..7623d1e 100644 --- a/gcode.c +++ b/gcode.c @@ -39,7 +39,9 @@ parser_state_t gc; #define FAIL(status) gc.status_code = status; -static int next_statement(char *letter, float *float_ptr, char *line, uint8_t *char_counter); +static uint8_t next_statement(char *letter, float *float_ptr, char *line, uint8_t *char_counter); +static void gc_convert_arc_radius_mode(float *target) __attribute__((noinline)); + static void select_plane(uint8_t axis_0, uint8_t axis_1, uint8_t axis_2) { @@ -48,6 +50,7 @@ static void select_plane(uint8_t axis_0, uint8_t axis_1, uint8_t axis_2) gc.plane_axis_2 = axis_2; } + void gc_init() { memset(&gc, 0, sizeof(gc)); @@ -61,20 +64,24 @@ void gc_init() } } + // Sets g-code parser position in mm. Input in steps. Called by the system abort and hard // limit pull-off routines. -void gc_set_current_position(int32_t x, int32_t y, int32_t z) +void gc_sync_position(int32_t x, int32_t y, int32_t z) { - gc.position[X_AXIS] = x/settings.steps_per_mm[X_AXIS]; - gc.position[Y_AXIS] = y/settings.steps_per_mm[Y_AXIS]; - gc.position[Z_AXIS] = z/settings.steps_per_mm[Z_AXIS]; + uint8_t i; + for (i=0; i C -----------------+--------------- T <- [x,y] - | <------ d/2 ---->| - - C - Current position - T - Target position - O - center of circle that pass through both C and T - d - distance from C to T - r - designated radius - h - distance from center of CT to O - - Expanding the equations: - - d -> sqrt(x^2 + y^2) - h -> sqrt(4 * r^2 - x^2 - y^2)/2 - i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 - j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 - - Which can be written: - - i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2 - j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2 - - Which we for size and speed reasons optimize to: - - h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2) - i = (x - (y * h_x2_div_d))/2 - j = (y + (x * h_x2_div_d))/2 - - */ - - // Calculate the change in position along each selected axis - float x = target[gc.plane_axis_0]-gc.position[gc.plane_axis_0]; - float y = target[gc.plane_axis_1]-gc.position[gc.plane_axis_1]; - - clear_vector(offset); - // First, use h_x2_div_d to compute 4*h^2 to check if it is negative or r is smaller - // than d. If so, the sqrt of a negative number is complex and error out. - float h_x2_div_d = 4 * r*r - x*x - y*y; - if (h_x2_div_d < 0) { FAIL(STATUS_ARC_RADIUS_ERROR); return(gc.status_code); } - // Finish computing h_x2_div_d. - h_x2_div_d = -sqrt(h_x2_div_d)/hypot(x,y); // == -(h * 2 / d) - // Invert the sign of h_x2_div_d if the circle is counter clockwise (see sketch below) - if (gc.motion_mode == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d; } - - /* The counter clockwise circle lies to the left of the target direction. When offset is positive, - the left hand circle will be generated - when it is negative the right hand circle is generated. - - - T <-- Target position - - ^ - Clockwise circles with this center | Clockwise circles with this center will have - will have > 180 deg of angular travel | < 180 deg of angular travel, which is a good thing! - \ | / - center of arc when h_x2_div_d is positive -> x <----- | -----> x <- center of arc when h_x2_div_d is negative - | - | - - C <-- Current position */ - - - // Negative R is g-code-alese for "I want a circle with more than 180 degrees of travel" (go figure!), - // 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; - 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] = 0.5*(x-(y*h_x2_div_d)); - offset[gc.plane_axis_1] = 0.5*(y+(x*h_x2_div_d)); - + if (gc.arc_radius != 0) { // Arc Radius Mode + // Compute arc radius and offsets + gc_convert_arc_radius_mode(target); + if (gc.status_code) { return(gc.status_code); } } else { // Arc Center Format Offset Mode - r = hypot(offset[gc.plane_axis_0], offset[gc.plane_axis_1]); // Compute arc radius for mc_arc + gc.arc_radius = hypot(gc.arc_offset[gc.plane_axis_0], gc.arc_offset[gc.plane_axis_1]); // Compute arc radius for mc_arc } // Set clockwise/counter-clockwise sign for mc_arc computations @@ -523,9 +443,9 @@ uint8_t gc_execute_line(char *line) if (gc.motion_mode == MOTION_MODE_CW_ARC) { isclockwise = true; } // Trace the arc - mc_arc(gc.position, target, offset, gc.plane_axis_0, gc.plane_axis_1, gc.plane_axis_2, + mc_arc(gc.position, target, gc.arc_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, - r, isclockwise); + gc.arc_radius, isclockwise); } break; } @@ -557,7 +477,7 @@ uint8_t gc_execute_line(char *line) // Parses the next statement and leaves the counter on the first character following // the statement. Returns 1 if there was a statements, 0 if end of string was reached // or there was an error (check state.status_code). -static int next_statement(char *letter, float *float_ptr, char *line, uint8_t *char_counter) +static uint8_t next_statement(char *letter, float *float_ptr, char *line, uint8_t *char_counter) { if (line[*char_counter] == 0) { return(0); // No more statements @@ -576,6 +496,100 @@ static int next_statement(char *letter, float *float_ptr, char *line, uint8_t *c return(1); } + +static void gc_convert_arc_radius_mode(float *target) +{ +/* We need to calculate the center of the circle that has the designated radius and passes + through both the current position and the target position. This method calculates the following + set of equations where [x,y] is the vector from current to target position, d == magnitude of + that vector, h == hypotenuse of the triangle formed by the radius of the circle, the distance to + the center of the travel vector. A vector perpendicular to the travel vector [-y,x] is scaled to the + length of h [-y/d*h, x/d*h] and added to the center of the travel vector [x/2,y/2] to form the new point + [i,j] at [x/2-y/d*h, y/2+x/d*h] which will be the center of our arc. + + d^2 == x^2 + y^2 + h^2 == r^2 - (d/2)^2 + i == x/2 - y/d*h + j == y/2 + x/d*h + + O <- [i,j] + - | + r - | + - | + - | h + - | + [0,0] -> C -----------------+--------------- T <- [x,y] + | <------ d/2 ---->| + + C - Current position + T - Target position + O - center of circle that pass through both C and T + d - distance from C to T + r - designated radius + h - distance from center of CT to O + + Expanding the equations: + + d -> sqrt(x^2 + y^2) + h -> sqrt(4 * r^2 - x^2 - y^2)/2 + i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 + j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 + + Which can be written: + + i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2 + j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2 + + Which we for size and speed reasons optimize to: + + h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2) + i = (x - (y * h_x2_div_d))/2 + j = (y + (x * h_x2_div_d))/2 */ + + // Calculate the change in position along each selected axis + float x = target[gc.plane_axis_0]-gc.position[gc.plane_axis_0]; + float y = target[gc.plane_axis_1]-gc.position[gc.plane_axis_1]; + + clear_vector(gc.arc_offset); + // First, use h_x2_div_d to compute 4*h^2 to check if it is negative or r is smaller + // than d. If so, the sqrt of a negative number is complex and error out. + float h_x2_div_d = 4 * gc.arc_radius*gc.arc_radius - x*x - y*y; + if (h_x2_div_d < 0) { FAIL(STATUS_ARC_RADIUS_ERROR); return; } + // Finish computing h_x2_div_d. + h_x2_div_d = -sqrt(h_x2_div_d)/hypot(x,y); // == -(h * 2 / d) + // Invert the sign of h_x2_div_d if the circle is counter clockwise (see sketch below) + if (gc.motion_mode == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d; } + + /* The counter clockwise circle lies to the left of the target direction. When offset is positive, + the left hand circle will be generated - when it is negative the right hand circle is generated. + + + T <-- Target position + + ^ + Clockwise circles with this center | Clockwise circles with this center will have + will have > 180 deg of angular travel | < 180 deg of angular travel, which is a good thing! + \ | / + center of arc when h_x2_div_d is positive -> x <----- | -----> x <- center of arc when h_x2_div_d is negative + | + | + + C <-- Current position */ + + + // Negative R is g-code-alese for "I want a circle with more than 180 degrees of travel" (go figure!), + // 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 (gc.arc_radius < 0) { + h_x2_div_d = -h_x2_div_d; + gc.arc_radius = -gc.arc_radius; // Finished with r. Set to positive for mc_arc + } + // Complete the operation by calculating the actual center of the arc + gc.arc_offset[gc.plane_axis_0] = 0.5*(x-(y*h_x2_div_d)); + gc.arc_offset[gc.plane_axis_1] = 0.5*(y+(x*h_x2_div_d)); +} + /* Not supported: diff --git a/gcode.h b/gcode.h index 50b4228..6b8949a 100644 --- a/gcode.h +++ b/gcode.h @@ -82,7 +82,11 @@ typedef struct { float coord_system[N_AXIS]; // Current work coordinate system (G54+). Stores offset from absolute machine // position in mm. Loaded from EEPROM when called. float coord_offset[N_AXIS]; // Retains the G92 coordinate offset (work coordinates) relative to - // machine zero in mm. Non-persistent. Cleared upon reset and boot. + // machine zero in mm. Non-persistent. Cleared upon reset and boot. + + float arc_radius; + float arc_offset[N_AXIS]; + } parser_state_t; extern parser_state_t gc; @@ -93,6 +97,6 @@ void gc_init(); uint8_t gc_execute_line(char *line); // Set g-code parser position. Input in steps. -void gc_set_current_position(int32_t x, int32_t y, int32_t z); +void gc_sync_position(); #endif diff --git a/limits.c b/limits.c index 3507785..a03d32e 100644 --- a/limits.c +++ b/limits.c @@ -96,16 +96,17 @@ static void homing_cycle(uint8_t cycle_mask, int8_t pos_dir, bool invert_pin, fl // and speedy homing routine. // NOTE: For each axes enabled, the following calculations assume they physically move // an equal distance over each time step until they hit a limit switch, aka dogleg. - uint32_t step_event_count, steps[N_AXIS]; + uint32_t step_event_count = 0; uint8_t i, dist = 0; + uint32_t steps[N_AXIS]; clear_vector(steps); for (i=0; i 0 || target[X_AXIS] < -settings.max_travel[X_AXIS] || - target[Y_AXIS] > 0 || target[Y_AXIS] < -settings.max_travel[Y_AXIS] || - target[Z_AXIS] > 0 || target[Z_AXIS] < -settings.max_travel[Z_AXIS] ) { - - // Force feed hold if cycle is active. All buffered blocks are guaranteed to be within - // workspace volume so just come to a controlled stop so position is not lost. When complete - // enter alarm mode. - if (sys.state == STATE_CYCLE) { - st_feed_hold(); - while (sys.state == STATE_HOLD) { - protocol_execute_runtime(); - if (sys.abort) { return; } - } - } + uint8_t idx; + for (idx=0; idx 0 || target[idx] < settings.max_travel[idx]) { // NOTE: max_travel is stored as negative - mc_reset(); // Issue system reset and ensure spindle and coolant are shutdown. - sys.execute |= EXEC_CRIT_EVENT; // Indicate soft limit critical event - protocol_execute_runtime(); // Execute to enter critical event loop and system abort + // Force feed hold if cycle is active. All buffered blocks are guaranteed to be within + // workspace volume so just come to a controlled stop so position is not lost. When complete + // enter alarm mode. + if (sys.state == STATE_CYCLE) { + st_feed_hold(); + while (sys.state == STATE_HOLD) { + protocol_execute_runtime(); + if (sys.abort) { return; } + } + } + + mc_reset(); // Issue system reset and ensure spindle and coolant are shutdown. + sys.execute |= EXEC_CRIT_EVENT; // Indicate soft limit critical event + protocol_execute_runtime(); // Execute to enter critical event loop and system abort + return; + + } } } diff --git a/main.c b/main.c index 84b0313..9decae8 100644 --- a/main.c +++ b/main.c @@ -72,7 +72,8 @@ int main(void) // Sync cleared gcode and planner positions to current system position, which is only // cleared upon startup, not a reset/abort. - sys_sync_current_position(); + plan_sync_position(); + gc_sync_position(); // Reset system variables. sys.abort = false; @@ -101,12 +102,12 @@ int main(void) } protocol_execute_runtime(); - protocol_process(); // ... process the serial protocol // When the serial protocol returns, there are no more characters in the serial read buffer to // be processed and executed. This indicates that individual commands are being issued or // streaming is finished. In either case, auto-cycle start, if enabled, any queued moves. mc_auto_cycle_start(); + protocol_process(); // ... process the serial protocol } return 0; /* never reached */ diff --git a/motion_control.c b/motion_control.c index 2a2f508..e420c98 100644 --- a/motion_control.c +++ b/motion_control.c @@ -72,7 +72,7 @@ void mc_line(float *target, float feed_rate, uint8_t invert_feed_rate) else { break; } } while (1); - plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], feed_rate, invert_feed_rate); + plan_buffer_line(target, feed_rate, invert_feed_rate); // If idle, indicate to the system there is now a planned block in the buffer ready to cycle // start. Otherwise ignore and continue on. @@ -148,8 +148,8 @@ void mc_arc(float *position, float *target, float *offset, uint8_t axis_0, uint8 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.16666667*(cos_T + 4); + float cos_T = 2.0 - theta_per_segment*theta_per_segment; + float sin_T = theta_per_segment*0.16666667*(cos_T + 4.0); cos_T *= 0.5; float arc_target[N_AXIS]; @@ -229,37 +229,34 @@ void mc_go_home() // At the same time, set up pull-off maneuver from axes limit switches that have been homed. // This provides some initial clearance off the switches and should also help prevent them // from falsely tripping when hard limits are enabled. - // TODO: Need to improve dir_mask[] to be more axes independent. float pulloff_target[N_AXIS]; clear_vector_float(pulloff_target); // Zero pulloff target. clear_vector_long(sys.position); // Zero current position for now. - uint8_t dir_mask[N_AXIS]; - dir_mask[X_AXIS] = (1< + - +--------+ <- nominal_rate /|\ - / \ / | \ - initial_rate -> + \ / | + <- next->initial_rate - | + <- next->initial_rate / | | - +-------------+ initial_rate -> +----+--+ - time --> ^ ^ ^ ^ - | | | | - decelerate distance decelerate distance - - Calculates trapezoid parameters for stepper algorithm. Each block velocity profiles can be - described as either a trapezoidal or a triangular shape. The trapezoid occurs when the block - reaches the nominal speed of the block and cruises for a period of time. A triangle occurs - when the nominal speed is not reached within the block. Some other special cases exist, - such as pure ac/de-celeration velocity profiles from beginning to end or a trapezoid that - has no deceleration period when the next block resumes acceleration. - - The following function determines the type of velocity profile and stores the minimum required - information for the stepper algorithm to execute the calculated profiles. In this case, only - the new initial rate and n_steps until deceleration are computed, since the stepper algorithm - already handles acceleration and cruising and just needs to know when to start decelerating. -*/ -static void calculate_trapezoid_for_block(block_t *block, float entry_speed_sqr, float exit_speed_sqr) -{ - // Compute new initial rate for stepper algorithm - block->initial_rate = ceil(sqrt(entry_speed_sqr)*(RANADE_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) - - // TODO: Compute new nominal rate if a feedrate override occurs. - // block->nominal_rate = ceil(feed_rate*(RANADE_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) - - // Compute efficiency variable for following calculations. Removes a float divide and multiply. - // TODO: If memory allows, this can be kept in the block buffer since it doesn't change, even after feed holds. - float steps_per_mm_div_2_acc = block->step_event_count/(2*block->acceleration*block->millimeters); - - // First determine intersection distance (in steps) from the exit point for a triangular profile. - // Computes: steps_intersect = steps/mm * ( distance/2 + (v_entry^2-v_exit^2)/(4*acceleration) ) - int32_t intersect_distance = ceil( 0.5*(block->step_event_count + steps_per_mm_div_2_acc*(entry_speed_sqr-exit_speed_sqr)) ); - - // Check if this is a pure acceleration block by a intersection distance less than zero. Also - // prevents signed and unsigned integer conversion errors. - if (intersect_distance <= 0) { - block->decelerate_after = 0; - } else { - // Determine deceleration distance (in steps) from nominal speed to exit speed for a trapezoidal profile. - // Value is never negative. Nominal speed is always greater than or equal to the exit speed. - // Computes: steps_decelerate = steps/mm * ( (v_nominal^2 - v_exit^2)/(2*acceleration) ) - block->decelerate_after = ceil(steps_per_mm_div_2_acc * (block->nominal_speed_sqr - exit_speed_sqr)); - - // The lesser of the two triangle and trapezoid distances always defines the velocity profile. - if (block->decelerate_after > intersect_distance) { block->decelerate_after = intersect_distance; } - - // Finally, check if this is a pure deceleration block. - if (block->decelerate_after > block->step_event_count) { block->decelerate_after = block->step_event_count; } - } -} - + /* PLANNER SPEED DEFINITION +--------+ <- current->nominal_speed @@ -185,178 +126,130 @@ static void calculate_trapezoid_for_block(block_t *block, float entry_speed_sqr, */ static void planner_recalculate() { - -// float entry_speed_sqr; -// uint8_t block_index = block_buffer_head; -// block_t *previous = NULL; -// block_t *current = NULL; -// block_t *next; -// while (block_index != block_buffer_tail) { -// block_index = prev_block_index( block_index ); -// next = current; -// current = previous; -// previous = &block_buffer[block_index]; -// -// if (next && current) { -// if (next != block_buffer_planned) { -// if (previous == block_buffer_tail) { block_buffer_planned = next; } -// else { -// -// if (current->entry_speed_sqr != current->max_entry_speed_sqr) { -// current->recalculate_flag = true; // Almost always changes. So force recalculate. -// entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters; -// if (entry_speed_sqr < current->max_entry_speed_sqr) { -// current->entry_speed_sqr = entry_speed_sqr; -// } else { -// current->entry_speed_sqr = current->max_entry_speed_sqr; -// } -// } else { -// block_buffer_planned = current; -// } -// } -// } else { -// break; -// } -// } -// } -// -// block_index = block_buffer_planned; -// next = &block_buffer[block_index]; -// current = prev_block_index(block_index); -// while (block_index != block_buffer_head) { -// -// // If the current 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 exit speed accordingly. Entry -// // speeds have already been reset, maximized, and reverse planned by reverse planner. -// if (current->entry_speed_sqr < next->entry_speed_sqr) { -// // Compute block exit speed based on the current block speed and distance -// // Computes: v_exit^2 = v_entry^2 + 2*acceleration*distance -// entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters; -// -// // If it's less than the stored value, update the exit speed and set recalculate flag. -// if (entry_speed_sqr < next->entry_speed_sqr) { -// next->entry_speed_sqr = entry_speed_sqr; -// next->recalculate_flag = true; -// } -// } -// -// // Recalculate if current block entry or exit junction speed has changed. -// if (current->recalculate_flag || next->recalculate_flag) { -// // NOTE: Entry and exit factors always > 0 by all previous logic operations. -// calculate_trapezoid_for_block(current, current->entry_speed_sqr, next->entry_speed_sqr); -// current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed -// } -// -// current = next; -// next = &block_buffer[block_index]; -// block_index = next_block_index( block_index ); -// } -// -// // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated. -// calculate_trapezoid_for_block(next, next->entry_speed_sqr, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED); -// next->recalculate_flag = false; + // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated. + uint8_t block_index = block_buffer_head; + plan_block_t *current = &block_buffer[block_index]; // Set as last/newest block in buffer - // TODO: No over-write protection exists for the executing block. For most cases this has proven to be ok, but - // for feed-rate overrides, something like this is essential. Place a request here to the stepper driver to - // find out where in the planner buffer is the a safe place to begin re-planning from. + // Ping the stepper algorithm to check if we can alter the parameters of the currently executing + // block. If not, skip it and work on the next block. + // TODO: Need to look into if there are conditions where this fails. + uint8_t block_buffer_safe = next_block_index( block_buffer_tail ); + + // TODO: Need to recompute buffer tail millimeters based on how much is completed. + + if (block_buffer_safe == next_buffer_head) { // Only one safe block in buffer to operate on -// if (block_buffer_head != block_buffer_tail) { - float entry_speed_sqr; + block_buffer_planned = block_buffer_safe; +// calculate_trapezoid_for_block(current, 0.0, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED); - // Perform reverse planner pass. Skip the head(end) block since it is already initialized, and skip the - // tail(first) block to prevent over-writing of the initial entry speed. - uint8_t block_index = prev_block_index( block_buffer_head ); // Assume buffer is not empty. - block_t *current = &block_buffer[block_index]; // Head block-1 = Newly appended block - block_t *next; - if (block_index != block_buffer_tail) { block_index = prev_block_index( block_index ); } - while (block_index != block_buffer_tail) { - next = current; - current = &block_buffer[block_index]; - - // TODO: Determine maximum entry speed at junction for feedrate overrides, since they can alter - // the planner nominal speeds at any time. This calc could be done in the override handler, but - // this could require an additional variable to be stored to differentiate the programmed nominal - // speeds, max junction speed, and override speeds/scalar. - - // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising. - // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and - // check for maximum allowable speed reductions to ensure maximum possible planned speed. - if (current->entry_speed_sqr != current->max_entry_speed_sqr) { + } else { + + // TODO: need to account for the two block condition better. If the currently executing block + // is not safe, do we wait until its done? Can we treat the buffer head differently? + + // Calculate trapezoid for the last/newest block. + current->entry_speed_sqr = min( current->max_entry_speed_sqr, + MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED + 2*current->acceleration*current->millimeters); +// calculate_trapezoid_for_block(current, current->entry_speed_sqr, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED); - current->entry_speed_sqr = current->max_entry_speed_sqr; - current->recalculate_flag = true; // Almost always changes. So force recalculate. - - if (next->entry_speed_sqr < current->max_entry_speed_sqr) { - // Computes: v_entry^2 = v_exit^2 + 2*acceleration*distance + + // Reverse Pass: Back plan the deceleration curve from the last block in buffer. Cease + // planning when: (1) the last optimal planned pointer is reached. (2) the safe block + // pointer is reached, whereby the planned pointer is updated. + float entry_speed_sqr; + plan_block_t *next; + block_index = prev_block_index(block_index); + while (block_index != block_buffer_planned) { + next = current; + current = &block_buffer[block_index]; + + // Exit loop and update planned pointer when the tail/safe block is reached. + if (block_index == block_buffer_safe) { + block_buffer_planned = block_buffer_safe; + break; + } + + // Crudely maximize deceleration curve from the end of the non-optimally planned buffer to + // the optimal plan pointer. Forward pass will adjust and finish optimizing the plan. + if (current->entry_speed_sqr != current->max_entry_speed_sqr) { entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters; if (entry_speed_sqr < current->max_entry_speed_sqr) { current->entry_speed_sqr = entry_speed_sqr; + } else { + current->entry_speed_sqr = current->max_entry_speed_sqr; } - } - } - block_index = prev_block_index( block_index ); - } + } + block_index = prev_block_index(block_index); + } - // Perform forward planner pass. Begins junction speed adjustments after tail(first) block. - // Also recalculate trapezoids, block by block, as the forward pass completes the plan. - block_index = next_block_index(block_buffer_tail); - next = &block_buffer[block_buffer_tail]; // Places tail(first) block into current - while (block_index != block_buffer_head) { - current = next; - next = &block_buffer[block_index]; - - // If the current 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 exit speed accordingly. Entry - // speeds have already been reset, maximized, and reverse planned by reverse planner. + // Forward Pass: Forward plan the acceleration curve from the planned pointer onward. + // Also scans for optimal plan breakpoints and appropriately updates the planned pointer. + block_index = block_buffer_planned; // Begin at buffer planned pointer + next = &block_buffer[prev_block_index(block_buffer_planned)]; // Set up for while loop + while (block_index != next_buffer_head) { + current = next; + next = &block_buffer[block_index]; + + // Any acceleration detected in the forward pass automatically moves the optimal planned + // pointer forward, since everything before this is all optimal. In other words, nothing + // can improve the plan from the buffer tail to the planned pointer by logic. if (current->entry_speed_sqr < next->entry_speed_sqr) { - // Compute block exit speed based on the current block speed and distance - // Computes: v_exit^2 = v_entry^2 + 2*acceleration*distance + block_buffer_planned = block_index; entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters; - - // If it's less than the stored value, update the exit speed and set recalculate flag. if (entry_speed_sqr < next->entry_speed_sqr) { - next->entry_speed_sqr = entry_speed_sqr; - next->recalculate_flag = true; + next->entry_speed_sqr = entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass set this. } } - - // Recalculate if current block entry or exit junction speed has changed. - if (current->recalculate_flag || next->recalculate_flag) { - // NOTE: Entry and exit factors always > 0 by all previous logic operations. - calculate_trapezoid_for_block(current, current->entry_speed_sqr, next->entry_speed_sqr); - current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed + + // Any block set at its maximum entry speed also creates an optimal plan up to this + // point in the buffer. The optimally planned pointer is updated. + if (next->entry_speed_sqr == next->max_entry_speed_sqr) { + block_buffer_planned = block_index; } - - block_index = next_block_index( block_index ); - } + + // Automatically recalculate trapezoid for all buffer blocks from last plan's optimal planned + // pointer to the end of the buffer, except the last block. +// calculate_trapezoid_for_block(current, current->entry_speed_sqr, next->entry_speed_sqr); + block_index = next_block_index( block_index ); + } + + } - // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated. - calculate_trapezoid_for_block(next, next->entry_speed_sqr, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED); - next->recalculate_flag = false; -// } } + void plan_init() { + block_buffer_head = 0; block_buffer_tail = block_buffer_head; next_buffer_head = next_block_index(block_buffer_head); -// block_buffer_planned = block_buffer_head; + block_buffer_planned = block_buffer_head; memset(&pl, 0, sizeof(pl)); // Clear planner struct } -inline void plan_discard_current_block() + +void plan_discard_current_block() { if (block_buffer_head != block_buffer_tail) { block_buffer_tail = next_block_index( block_buffer_tail ); } } -inline block_t *plan_get_current_block() + +plan_block_t *plan_get_current_block() { if (block_buffer_head == block_buffer_tail) { return(NULL); } return(&block_buffer[block_buffer_tail]); } + +plan_block_t *plan_get_block_by_index(uint8_t block_index) +{ + if (block_buffer_head == block_index) { return(NULL); } + return(&block_buffer[block_index]); +} + + // Returns the availability status of the block ring buffer. True, if full. uint8_t plan_check_full_buffer() { @@ -364,6 +257,7 @@ uint8_t plan_check_full_buffer() return(false); } + // Block until all buffered steps are executed or in a cycle state. Works with feed hold // during a synchronize call, if it should happen. Also, waits for clean cycle end. void plan_synchronize() @@ -374,43 +268,54 @@ void plan_synchronize() } } -// Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in -// millimeters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed + +// Add a new linear movement to the buffer. target[N_AXIS] is the signed, absolute target position +// in millimeters. 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. // All position data passed to the planner must be in terms of machine position to keep the planner // independent of any coordinate system changes and offsets, which are handled by the g-code parser. // NOTE: Assumes buffer is available. Buffer checks are handled at a higher level by motion_control. -// Also the feed rate input value is used in three ways: as a normal feed rate if invert_feed_rate -// is false, as inverse time if invert_feed_rate is true, or as seek/rapids rate if the feed_rate -// value is negative (and invert_feed_rate always false). -void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert_feed_rate) +// In other words, the buffer head is never equal to the buffer tail. Also the feed rate input value +// is used in three ways: as a normal feed rate if invert_feed_rate is false, as inverse time if +// invert_feed_rate is true, or as seek/rapids rate if the feed_rate value is negative (and +// invert_feed_rate always false). +void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) { - // Prepare to set up new block - block_t *block = &block_buffer[block_buffer_head]; + // Prepare and initialize new block + plan_block_t *block = &block_buffer[block_buffer_head]; + block->step_event_count = 0; + block->millimeters = 0; + block->direction_bits = 0; + block->acceleration = SOME_LARGE_VALUE; // Scaled down to maximum acceleration later - // Calculate target position in absolute steps - int32_t target[N_AXIS]; - target[X_AXIS] = lround(x*settings.steps_per_mm[X_AXIS]); - target[Y_AXIS] = lround(y*settings.steps_per_mm[Y_AXIS]); - target[Z_AXIS] = lround(z*settings.steps_per_mm[Z_AXIS]); + // Compute and store initial move distance data. + int32_t target_steps[N_AXIS]; + float unit_vec[N_AXIS], delta_mm; + uint8_t idx; + for (idx=0; idxsteps[idx] = labs(target_steps[idx]-pl.position[idx]); + block->step_event_count = max(block->step_event_count, block->steps[idx]); + + // Compute individual axes distance for move and prep unit vector calculations. + // NOTE: Computes true distance from converted step values. + delta_mm = (target_steps[idx] - pl.position[idx])/settings.steps_per_mm[idx]; + unit_vec[idx] = delta_mm; // Store unit vector numerator. Denominator computed later. + + // Set direction bits. Bit enabled always means direction is negative. + if (delta_mm < 0 ) { block->direction_bits |= get_direction_mask(idx); } + + // Incrementally compute total move distance by Euclidean norm. First add square of each term. + block->millimeters += delta_mm*delta_mm; + } + block->millimeters = sqrt(block->millimeters); // Complete millimeters calculation with sqrt() - // Number of steps for each axis - block->steps_x = labs(target[X_AXIS]-pl.position[X_AXIS]); - block->steps_y = labs(target[Y_AXIS]-pl.position[Y_AXIS]); - block->steps_z = labs(target[Z_AXIS]-pl.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; }; + if (block->step_event_count == 0) { return; } - // Compute path vector in terms of absolute step target and current positions - float delta_mm[N_AXIS]; - delta_mm[X_AXIS] = x-pl.last_x; - delta_mm[Y_AXIS] = y-pl.last_y; - delta_mm[Z_AXIS] = z-pl.last_z; - block->millimeters = sqrt(delta_mm[X_AXIS]*delta_mm[X_AXIS] + delta_mm[Y_AXIS]*delta_mm[Y_AXIS] + - delta_mm[Z_AXIS]*delta_mm[Z_AXIS]); - // Adjust feed_rate value to mm/min depending on type of rate input (normal, inverse time, or rapids) // TODO: Need to distinguish a rapids vs feed move for overrides. Some flag of some sort. if (feed_rate < 0) { feed_rate = SOME_LARGE_VALUE; } // Scaled down to absolute max/rapids rate later @@ -422,117 +327,151 @@ void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert // and axes properties as well. // NOTE: This calculation assumes all axes are orthogonal (Cartesian) and works with ABC-axes, // if they are also orthogonal/independent. Operates on the absolute value of the unit vector. - uint8_t i; - float unit_vec[N_AXIS], inverse_unit_vec_value; + float inverse_unit_vec_value; float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple float divides - block->acceleration = SOME_LARGE_VALUE; // Scaled down to maximum acceleration in loop - for (i=0; iacceleration = min(block->acceleration,settings.acceleration[i]*inverse_unit_vec_value); + float junction_cos_theta = 0; + for (idx=0; idxacceleration = min(block->acceleration,settings.acceleration[idx]*inverse_unit_vec_value); + + // Incrementally compute cosine of angle between previous and current path. Cos(theta) of the junction + // between the current move and the previous move is simply the dot product of the two unit vectors, + // where prev_unit_vec is negative. Used later to compute maximum junction speed. + junction_cos_theta -= pl.previous_unit_vec[idx] * unit_vec[idx]; } } - // Compute nominal speed and rates - block->nominal_speed_sqr = feed_rate*feed_rate; // (mm/min)^2. Always > 0 - block->nominal_rate = ceil(feed_rate*(RANADE_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + /* Compute maximum allowable entry speed at junction by centripetal acceleration approximation. + 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 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. This approach does not actually deviate + from path, but used as a robust way to compute cornering speeds, as it takes into account the + nonlinearities of both the junction angle and junction velocity. + NOTE: If the junction deviation value is finite, Grbl executes the motions in an exact path + mode (G61). If the junction deviation value is zero, Grbl will execute the motion in an exact + stop mode (G61.1) manner. In the future, if continuous mode (G64) is desired, the math here + is exactly the same. Instead of motioning all the way to junction point, the machine will + just follow the arc circle defined here. The Arduino doesn't have the CPU cycles to perform + a continuous mode path, but ARM-based microcontrollers most certainly do. + */ + // TODO: Acceleration need to be limited by the minimum of the two junctions. + // TODO: Need to setup a method to handle zero junction speeds when starting from rest. + if (block_buffer_head == block_buffer_tail) { + block->max_entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED; + } else { + // NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta). + float sin_theta_d2 = sqrt(0.5*(1.0-junction_cos_theta)); // Trig half angle identity. Always positive. + block->max_entry_speed_sqr = (block->acceleration * settings.junction_deviation * sin_theta_d2)/(1.0-sin_theta_d2); + } - // Compute the acceleration and distance traveled per step event for the stepper algorithm. - block->rate_delta = ceil(block->acceleration* - ((RANADE_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) - block->d_next = ceil((block->millimeters*RANADE_MULTIPLIER)/block->step_event_count); // (mult*mm/step) - - // Compute direction bits. Bit enabled always means direction is negative. - block->direction_bits = 0; - if (unit_vec[X_AXIS] < 0) { block->direction_bits |= (1<direction_bits |= (1<direction_bits |= (1<max_entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED; - if ((block_buffer_head != block_buffer_tail) && (pl.previous_nominal_speed_sqr > 0.0)) { - // 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. - float cos_theta = - pl.previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] - - pl.previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] - - pl.previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ; - - // Skip and use default max junction speed for 0 degree acute junction. - if (cos_theta < 0.95) { - block->max_entry_speed_sqr = min(block->nominal_speed_sqr,pl.previous_nominal_speed_sqr); - // Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds. - if (cos_theta > -0.95) { - // Compute maximum junction velocity based on maximum acceleration and junction deviation - float sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive. - block->max_entry_speed_sqr = min(block->max_entry_speed_sqr, - block->acceleration * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)); - } - } - } - - // Initialize block entry speed. Compute block entry velocity backwards from user-defined MINIMUM_PLANNER_SPEED. - // TODO: This could be moved to the planner recalculate function. - block->entry_speed_sqr = min( block->max_entry_speed_sqr, - MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED + 2*block->acceleration*block->millimeters); - - // Set new block to be recalculated for conversion to stepper data. - block->recalculate_flag = true; + // Store block nominal speed and rate + block->nominal_speed_sqr = feed_rate*feed_rate; // (mm/min). Always > 0 +// block->nominal_rate = ceil(feed_rate*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) +// +// // Compute and store acceleration and distance traveled per step event. +// block->rate_delta = ceil(block->acceleration* +// ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) +// block->d_next = ceil((block->millimeters*INV_TIME_MULTIPLIER)/block->step_event_count); // (mult*mm/step) // Update previous path unit_vector and nominal speed (squared) memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // pl.previous_unit_vec[] = unit_vec[] pl.previous_nominal_speed_sqr = block->nominal_speed_sqr; // Update planner position - memcpy(pl.position, target, sizeof(target)); // pl.position[] = target[] - pl.last_x = x; - pl.last_y = y; - pl.last_z = z; - - // Update buffer head and next buffer head indices - block_buffer_head = next_buffer_head; - next_buffer_head = next_block_index(block_buffer_head); + memcpy(pl.position, target_steps, sizeof(target_steps)); // pl.position[] = target_steps[] planner_recalculate(); + + // Update buffer head and next buffer head indices. + // NOTE: The buffer head update is atomic since it's one byte. Performed after the new plan + // calculations to help prevent overwriting scenarios with adding a new block to a low buffer. + block_buffer_head = next_buffer_head; + next_buffer_head = next_block_index(block_buffer_head); } + // Reset the planner position vectors. Called by the system abort/initialization routine. -void plan_set_current_position(int32_t x, int32_t y, int32_t z) +void plan_sync_position() { - pl.position[X_AXIS] = x; - pl.position[Y_AXIS] = y; - pl.position[Z_AXIS] = z; - pl.last_x = x/settings.steps_per_mm[X_AXIS]; - pl.last_y = y/settings.steps_per_mm[Y_AXIS]; - pl.last_z = z/settings.steps_per_mm[Z_AXIS]; + uint8_t idx; + for (idx=0; idx + + +--------+ <- nominal_rate /|\ + / \ / | \ + initial_rate -> + \ / | + <- next->initial_rate + | + <- next->initial_rate / | | + +-------------+ initial_rate -> +----+--+ + time --> ^ ^ ^ ^ + | | | | + decelerate distance decelerate distance + + Calculates trapezoid parameters for stepper algorithm. Each block velocity profiles can be + described as either a trapezoidal or a triangular shape. The trapezoid occurs when the block + reaches the nominal speed of the block and cruises for a period of time. A triangle occurs + when the nominal speed is not reached within the block. Some other special cases exist, + such as pure ac/de-celeration velocity profiles from beginning to end or a trapezoid that + has no deceleration period when the next block resumes acceleration. + + The following function determines the type of velocity profile and stores the minimum required + information for the stepper algorithm to execute the calculated profiles. In this case, only + the new initial rate and n_steps until deceleration are computed, since the stepper algorithm + already handles acceleration and cruising and just needs to know when to start decelerating. +*/ +int32_t calculate_trapezoid_for_block(uint8_t block_index) +{ + plan_block_t *current_block = &block_buffer[block_index]; + + // Determine current block exit speed + float exit_speed_sqr; + uint8_t next_index = next_block_index(block_index); + plan_block_t *next_block = plan_get_block_by_index(next_index); + if (next_block == NULL) { exit_speed_sqr = 0; } // End of planner buffer. Zero speed. + else { exit_speed_sqr = next_block->entry_speed_sqr; } // Entry speed of next block + + // First determine intersection distance (in steps) from the exit point for a triangular profile. + // Computes: steps_intersect = steps/mm * ( distance/2 + (v_entry^2-v_exit^2)/(4*acceleration) ) + float intersect_distance = 0.5*( current_block->millimeters + (current_block->entry_speed_sqr-exit_speed_sqr)/(2*current_block->acceleration) ); + + // Check if this is a pure acceleration block by a intersection distance less than zero. Also + // prevents signed and unsigned integer conversion errors. + if (intersect_distance > 0 ) { + float decelerate_distance; + // Determine deceleration distance (in steps) from nominal speed to exit speed for a trapezoidal profile. + // Value is never negative. Nominal speed is always greater than or equal to the exit speed. + // Computes: steps_decelerate = steps/mm * ( (v_nominal^2 - v_exit^2)/(2*acceleration) ) + decelerate_distance = (current_block->nominal_speed_sqr - exit_speed_sqr)/(2*current_block->acceleration); + + // The lesser of the two triangle and trapezoid distances always defines the velocity profile. + if (decelerate_distance > intersect_distance) { decelerate_distance = intersect_distance; } + + // Finally, check if this is a pure deceleration block. + if (decelerate_distance > current_block->millimeters) { decelerate_distance = current_block->millimeters; } + + return(ceil(((current_block->millimeters-decelerate_distance)*current_block->step_event_count)/ current_block->millimeters)); + } + return(0); +} + + // Re-initialize buffer plan with a partially completed block, assumed to exist at the buffer tail. // Called after a steppers have come to a complete stop for a feed hold and the cycle is stopped. void plan_cycle_reinitialize(int32_t step_events_remaining) { - block_t *block = &block_buffer[block_buffer_tail]; // Point to partially completed block + plan_block_t *block = &block_buffer[block_buffer_tail]; // Point to partially completed block // Only remaining millimeters and step_event_count need to be updated for planner recalculate. // Other variables (step_x, step_y, step_z, rate_delta, etc.) all need to remain the same to @@ -540,9 +479,9 @@ void plan_cycle_reinitialize(int32_t step_events_remaining) block->millimeters = (block->millimeters*step_events_remaining)/block->step_event_count; block->step_event_count = step_events_remaining; - // Re-plan from a complete stop. Reset planner entry speeds and flags. + // Re-plan from a complete stop. Reset planner entry speeds and buffer planned pointer. block->entry_speed_sqr = 0.0; - block->max_entry_speed_sqr = 0.0; - block->recalculate_flag = true; + block->max_entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED; + block_buffer_planned = block_buffer_tail; planner_recalculate(); } diff --git a/planner.h b/planner.h index 830735d..e371bb0 100644 --- a/planner.h +++ b/planner.h @@ -2,8 +2,8 @@ planner.h - buffers movement commands and manages the acceleration profile plan Part of Grbl + Copyright (c) 2011-2013 Sungeun K. Jeon Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2011-2013 Sungeun K. 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 @@ -21,10 +21,11 @@ #ifndef planner_h #define planner_h - +#include "nuts_bolts.h" + // The number of linear motions that can be in the plan at any give time #ifndef BLOCK_BUFFER_SIZE - #define BLOCK_BUFFER_SIZE 17 + #define BLOCK_BUFFER_SIZE 18 #endif // This struct is used when buffering the setup for each linear movement "nominal" values are as specified in @@ -32,43 +33,43 @@ typedef struct { // Fields used by the bresenham algorithm for tracing the line - uint8_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) - uint32_t steps_x, steps_y, steps_z; // Step count along each axis - int32_t step_event_count; // The number of step events required to complete this block + uint8_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) + int32_t steps[N_AXIS]; // Step count along each axis + int32_t step_event_count; // The number of step events required to complete this block // Fields used by the motion planner to manage acceleration - float nominal_speed_sqr; // The nominal speed for this block in mm/min - float entry_speed_sqr; // Entry speed at previous-current block junction in mm/min - float max_entry_speed_sqr; // Maximum allowable junction entry speed in mm/min - float millimeters; // The total travel of this block in mm - float acceleration; - uint8_t recalculate_flag; // Planner flag to recalculate trapezoids on entry junction - + float nominal_speed_sqr; // Axis-limit adjusted nominal speed for this block in (mm/min)^2 + float entry_speed_sqr; // Entry speed at previous-current block junction in (mm/min)^2 + float max_entry_speed_sqr; // Maximum allowable junction entry speed in (mm/min)^2 + float acceleration; // Axes-limit adjusted line acceleration in mm/min^2 + float millimeters; // The total travel for this block to be executed in mm + // Settings for the trapezoid generator - uint32_t initial_rate; // The step rate at start of block - int32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive) - uint32_t decelerate_after; // The index of the step event on which to start decelerating - uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute - uint32_t d_next; // Scaled distance to next step -} block_t; +// int32_t decelerate_after; // The index of the step event on which to start decelerating + +} plan_block_t; // Initialize the motion plan subsystem 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 +// Add a new linear movement to the buffer. target[N_AXIS] is the signed, absolute target position +// in millimeters. 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(float x, float y, float z, float feed_rate, uint8_t invert_feed_rate); +void plan_buffer_line(float *target, float 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. void plan_discard_current_block(); // Gets the current block. Returns NULL if buffer empty -block_t *plan_get_current_block(); +plan_block_t *plan_get_current_block(); + +plan_block_t *plan_get_block_by_index(uint8_t block_index); + +int32_t calculate_trapezoid_for_block(uint8_t block_index); // Reset the planner position vector (in steps) -void plan_set_current_position(int32_t x, int32_t y, int32_t z); +void plan_sync_position(); // Reinitialize plan with a partially completed block void plan_cycle_reinitialize(int32_t step_events_remaining); diff --git a/planner_old.c b/planner_old.c new file mode 100644 index 0000000..1cf6fb4 --- /dev/null +++ b/planner_old.c @@ -0,0 +1,476 @@ +/* + planner.c - buffers movement commands and manages the acceleration profile plan + Part of Grbl + + Copyright (c) 2011-2013 Sungeun K. Jeon + Copyright (c) 2009-2011 Simen Svale Skogsrud + Copyright (c) 2011 Jens Geisler + + 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 + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +/* The ring buffer implementation gleaned from the wiring_serial library by David A. Mellis. */ + +#include +#include +#include "planner.h" +#include "nuts_bolts.h" +#include "stepper.h" +#include "settings.h" +#include "config.h" +#include "protocol.h" + +#define SOME_LARGE_VALUE 1.0E+38 // Used by rapids and acceleration maximization calculations. Just needs + // to be larger than any feasible (mm/min)^2 or mm/sec^2 value. + +static block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instructions +static volatile uint8_t block_buffer_tail; // Index of the block to process now +static uint8_t block_buffer_head; // Index of the next block to be pushed +static uint8_t next_buffer_head; // Index of the next buffer head +static uint8_t block_buffer_planned; // Index of the optimally planned block + +// Define planner variables +typedef struct { + int32_t position[N_AXIS]; // The planner position of the tool in absolute steps. Kept separate + // from g-code position for movements requiring multiple line motions, + // i.e. arcs, canned cycles, and backlash compensation. + float previous_unit_vec[N_AXIS]; // Unit vector of previous path line segment + float previous_nominal_speed_sqr; // Nominal speed of previous path line segment + float last_target[N_AXIS]; // Target position of previous path line segment +} planner_t; +static planner_t pl; + + +// Returns the index of the next block in the ring buffer +// NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication. +static uint8_t next_block_index(uint8_t block_index) +{ + block_index++; + if (block_index == BLOCK_BUFFER_SIZE) { block_index = 0; } + return(block_index); +} + + +// Returns the index of the previous block in the ring buffer +static uint8_t prev_block_index(uint8_t block_index) +{ + if (block_index == 0) { block_index = BLOCK_BUFFER_SIZE; } + block_index--; + return(block_index); +} + + +/* STEPPER VELOCITY PROFILE DEFINITION + less than nominal rate-> + + +--------+ <- nominal_rate /|\ + / \ / | \ + initial_rate -> + \ / | + <- next->initial_rate + | + <- next->initial_rate / | | + +-------------+ initial_rate -> +----+--+ + time --> ^ ^ ^ ^ + | | | | + decelerate distance decelerate distance + + Calculates trapezoid parameters for stepper algorithm. Each block velocity profiles can be + described as either a trapezoidal or a triangular shape. The trapezoid occurs when the block + reaches the nominal speed of the block and cruises for a period of time. A triangle occurs + when the nominal speed is not reached within the block. Some other special cases exist, + such as pure ac/de-celeration velocity profiles from beginning to end or a trapezoid that + has no deceleration period when the next block resumes acceleration. + + The following function determines the type of velocity profile and stores the minimum required + information for the stepper algorithm to execute the calculated profiles. In this case, only + the new initial rate and n_steps until deceleration are computed, since the stepper algorithm + already handles acceleration and cruising and just needs to know when to start decelerating. +*/ +static void calculate_trapezoid_for_block(block_t *block, float entry_speed_sqr, float exit_speed_sqr) +{ + // Compute new initial rate for stepper algorithm + block->initial_rate = ceil(sqrt(entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + + // TODO: Compute new nominal rate if a feedrate override occurs. + // block->nominal_rate = ceil(feed_rate*(RANADE_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + + // Compute efficiency variable for following calculations. Removes a float divide and multiply. + // TODO: If memory allows, this can be kept in the block buffer since it doesn't change, even after feed holds. + float steps_per_mm_div_2_acc = block->step_event_count/(2*block->acceleration*block->millimeters); + + // First determine intersection distance (in steps) from the exit point for a triangular profile. + // Computes: steps_intersect = steps/mm * ( distance/2 + (v_entry^2-v_exit^2)/(4*acceleration) ) + int32_t intersect_distance = ceil( 0.5*(block->step_event_count + steps_per_mm_div_2_acc*(entry_speed_sqr-exit_speed_sqr)) ); + + // Check if this is a pure acceleration block by a intersection distance less than zero. Also + // prevents signed and unsigned integer conversion errors. + if (intersect_distance <= 0) { + block->decelerate_after = 0; + } else { + // Determine deceleration distance (in steps) from nominal speed to exit speed for a trapezoidal profile. + // Value is never negative. Nominal speed is always greater than or equal to the exit speed. + // Computes: steps_decelerate = steps/mm * ( (v_nominal^2 - v_exit^2)/(2*acceleration) ) + block->decelerate_after = ceil(steps_per_mm_div_2_acc * (block->nominal_speed_sqr - exit_speed_sqr)); + + // The lesser of the two triangle and trapezoid distances always defines the velocity profile. + if (block->decelerate_after > intersect_distance) { block->decelerate_after = intersect_distance; } + + // Finally, check if this is a pure deceleration block. + if (block->decelerate_after > block->step_event_count) { block->decelerate_after = block->step_event_count; } + } +} + + +/* PLANNER SPEED DEFINITION + +--------+ <- current->nominal_speed + / \ + current->entry_speed -> + \ + | + <- next->entry_speed + +-------------+ + time --> + + 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 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 acceleration limits. + c. The last (or newest appended) block is planned from a complete stop. + 2. Go over every block in chronological (forward) order and dial down junction speed values if + a. The speed increase within one block would require faster acceleration than the acceleration limits. + + When these stages are complete, all blocks have a junction entry speed that will allow all speed changes + to be performed using the overall limiting acceleration value, and where no junction speed is greater + than the max limit. In other words, it just computed the fastest possible velocity profile through all + buffered blocks, where the final buffered block is planned to come to a full stop when the buffer is fully + executed. Finally it will: + + 3. Convert the plan to data that the stepper algorithm needs. Only block trapezoids adjacent to a + a planner-modified junction speed with be updated, the others are assumed ok as is. + + All planner computations(1)(2) are performed in floating point to minimize numerical round-off errors. Only + when planned values are converted to stepper rate parameters(3), these are integers. If another motion block + is added while executing, the planner will re-plan and update the stored optimal velocity profile as it goes. + + Conceptually, the planner works like blowing up a balloon, where the balloon is the velocity profile. It's + constrained by the speeds at the beginning and end of the buffer, along with the maximum junction speeds and + nominal speeds of each block. Once a plan is computed, or balloon filled, this is the optimal velocity profile + through all of the motions in the buffer. Whenever a new block is added, this changes some of the limiting + conditions, or how the balloon is filled, so it has to be re-calculated to get the new optimal velocity profile. + + Also, since the planner only computes on what's in the planner buffer, some motions with lots of short line + segments, like arcs, may seem to move slow. This is because there simply isn't enough combined distance traveled + in the entire buffer to accelerate up to the nominal speed and then decelerate to a stop at the end of the + buffer. There are a few simple solutions to this: (1) Maximize the machine acceleration. The planner will be + able to compute higher speed profiles within the same combined distance. (2) Increase line segment(s) distance. + The more combined distance the planner has to use, the faster it can go. (3) Increase the MINIMUM_PLANNER_SPEED. + Not recommended. This will change what speed the planner plans to at the end of the buffer. Can lead to lost + steps when coming to a stop. (4) [BEST] Increase the planner buffer size. The more combined distance, the + bigger the balloon, or faster it can go. But this is not possible for 328p Arduinos because its limited memory + is already maxed out. Future ARM versions should not have this issue, with look-ahead planner blocks numbering + up to a hundred or more. + + NOTE: Since this function is constantly re-calculating for every new incoming block, it must be as efficient + as possible. For example, in situations like arc generation or complex curves, the short, rapid line segments + can execute faster than new blocks can be added, and the planner buffer will then starve and empty, leading + to weird hiccup-like jerky motions. +*/ +static void planner_recalculate() +{ + // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated. + uint8_t block_index = block_buffer_head; + block_t *current = &block_buffer[block_index]; // Set as last/newest block in buffer + + // Determine safe point for which to plan to. + uint8_t block_buffer_safe = next_block_index( block_buffer_tail ); + + if (block_buffer_safe == next_buffer_head) { // Only one safe block in buffer to operate on + + block_buffer_planned = block_buffer_safe; + calculate_trapezoid_for_block(current, 0.0, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED); + + } else { + + // TODO: need to account for the two block condition better. If the currently executing block + // is not safe, do we wait until its done? Can we treat the buffer head differently? + + // Calculate trapezoid for the last/newest block. + current->entry_speed_sqr = min( current->max_entry_speed_sqr, + MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED + 2*current->acceleration*current->millimeters); + calculate_trapezoid_for_block(current, current->entry_speed_sqr, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED); + + + // Reverse Pass: Back plan the deceleration curve from the last block in buffer. Cease + // planning when: (1) the last optimal planned pointer is reached. (2) the safe block + // pointer is reached, whereby the planned pointer is updated. + float entry_speed_sqr; + block_t *next; + block_index = prev_block_index(block_index); + while (block_index != block_buffer_planned) { + next = current; + current = &block_buffer[block_index]; + + // Exit loop and update planned pointer when the tail/safe block is reached. + if (block_index == block_buffer_safe) { + block_buffer_planned = block_buffer_safe; + break; + } + + // Crudely maximize deceleration curve from the end of the non-optimally planned buffer to + // the optimal plan pointer. Forward pass will adjust and finish optimizing the plan. + if (current->entry_speed_sqr != current->max_entry_speed_sqr) { + entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters; + if (entry_speed_sqr < current->max_entry_speed_sqr) { + current->entry_speed_sqr = entry_speed_sqr; + } else { + current->entry_speed_sqr = current->max_entry_speed_sqr; + } + } + block_index = prev_block_index(block_index); + } + + // Forward Pass: Forward plan the acceleration curve from the planned pointer onward. + // Also scans for optimal plan breakpoints and appropriately updates the planned pointer. + block_index = block_buffer_planned; // Begin at buffer planned pointer + next = &block_buffer[prev_block_index(block_buffer_planned)]; // Set up for while loop + while (block_index != next_buffer_head) { + current = next; + next = &block_buffer[block_index]; + + // Any acceleration detected in the forward pass automatically moves the optimal planned + // pointer forward, since everything before this is all optimal. In other words, nothing + // can improve the plan from the buffer tail to the planned pointer by logic. + if (current->entry_speed_sqr < next->entry_speed_sqr) { + block_buffer_planned = block_index; + entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters; + if (entry_speed_sqr < next->entry_speed_sqr) { + next->entry_speed_sqr = entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass set this. + } + } + + // Any block set at its maximum entry speed also creates an optimal plan up to this + // point in the buffer. The optimally planned pointer is updated. + if (next->entry_speed_sqr == next->max_entry_speed_sqr) { + block_buffer_planned = block_index; + } + + // Automatically recalculate trapezoid for all buffer blocks from last plan's optimal planned + // pointer to the end of the buffer, except the last block. + calculate_trapezoid_for_block(current, current->entry_speed_sqr, next->entry_speed_sqr); + block_index = next_block_index( block_index ); + } + + } + +} + + +void plan_init() +{ + block_buffer_tail = block_buffer_head; + next_buffer_head = next_block_index(block_buffer_head); + block_buffer_planned = block_buffer_head; + memset(&pl, 0, sizeof(pl)); // Clear planner struct +} + + +inline void plan_discard_current_block() +{ + if (block_buffer_head != block_buffer_tail) { + block_buffer_tail = next_block_index( block_buffer_tail ); + } +} + + +inline block_t *plan_get_current_block() +{ + if (block_buffer_head == block_buffer_tail) { return(NULL); } + return(&block_buffer[block_buffer_tail]); +} + + +// Returns the availability status of the block ring buffer. True, if full. +uint8_t plan_check_full_buffer() +{ + if (block_buffer_tail == next_buffer_head) { return(true); } + return(false); +} + + +// Block until all buffered steps are executed or in a cycle state. Works with feed hold +// during a synchronize call, if it should happen. Also, waits for clean cycle end. +void plan_synchronize() +{ + while (plan_get_current_block() || sys.state == STATE_CYCLE) { + protocol_execute_runtime(); // Check and execute run-time commands + if (sys.abort) { return; } // Check for system abort + } +} + + +// Add a new linear movement to the buffer. target[N_AXIS] is the signed, absolute target position +// in millimeters. 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. +// All position data passed to the planner must be in terms of machine position to keep the planner +// independent of any coordinate system changes and offsets, which are handled by the g-code parser. +// NOTE: Assumes buffer is available. Buffer checks are handled at a higher level by motion_control. +// In other words, the buffer head is never equal to the buffer tail. Also the feed rate input value +// is used in three ways: as a normal feed rate if invert_feed_rate is false, as inverse time if +// invert_feed_rate is true, or as seek/rapids rate if the feed_rate value is negative (and +// invert_feed_rate always false). +void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) +{ + // Prepare and initialize new block + block_t *block = &block_buffer[block_buffer_head]; + block->step_event_count = 0; + block->millimeters = 0; + block->direction_bits = 0; + block->acceleration = SOME_LARGE_VALUE; // Scaled down to maximum acceleration later + + // Compute and store initial move distance data. + int32_t target_steps[N_AXIS]; + float unit_vec[N_AXIS], delta_mm; + uint8_t idx; + for (idx=0; idxsteps[idx] = labs(target_steps[idx]-pl.position[idx]); + block->step_event_count = max(block->step_event_count, block->steps[idx]); + + // Compute individual axes distance for move and prep unit vector calculations. + delta_mm = target[idx] - pl.last_target[idx]; + unit_vec[idx] = delta_mm; // Store unit vector numerator. Denominator computed later. + + // Incrementally compute total move distance by Euclidean norm + block->millimeters += delta_mm*delta_mm; + + // Set direction bits. Bit enabled always means direction is negative. + if (delta_mm < 0 ) { block->direction_bits |= get_direction_mask(idx); } + } + block->millimeters = sqrt(block->millimeters); // Complete millimeters calculation + + // Bail if this is a zero-length block + if (block->step_event_count == 0) { return; } + + // Adjust feed_rate value to mm/min depending on type of rate input (normal, inverse time, or rapids) + // TODO: Need to distinguish a rapids vs feed move for overrides. Some flag of some sort. + if (feed_rate < 0) { feed_rate = SOME_LARGE_VALUE; } // Scaled down to absolute max/rapids rate later + else if (invert_feed_rate) { feed_rate = block->millimeters/feed_rate; } + + // Calculate the unit vector of the line move and the block maximum feed rate and acceleration limited + // by the maximum possible values. Block rapids rates are computed or feed rates are scaled down so + // they don't exceed the maximum axes velocities. The block acceleration is maximized based on direction + // and axes properties as well. + // NOTE: This calculation assumes all axes are orthogonal (Cartesian) and works with ABC-axes, + // if they are also orthogonal/independent. Operates on the absolute value of the unit vector. + float inverse_unit_vec_value; + float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple float divides + float junction_cos_theta = 0; + for (idx=0; idxacceleration = min(block->acceleration,settings.acceleration[idx]*inverse_unit_vec_value); + + // Incrementally compute cosine of angle between previous and current path. Cos(theta) of the junction + // between the current move and the previous move is simply the dot product of the two unit vectors, + // where prev_unit_vec is negative. Used later to compute maximum junction speed. + junction_cos_theta -= pl.previous_unit_vec[idx] * unit_vec[idx]; + } + } + + /* Compute maximum allowable entry speed at junction by centripetal acceleration approximation. + 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 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. This approach does not actually deviate + from path, but used as a robust way to compute cornering speeds, as it takes into account the + nonlinearities of both the junction angle and junction velocity. + NOTE: If the junction deviation value is finite, Grbl executes the motions in an exact path + mode (G61). If the junction deviation value is zero, Grbl will execute the motion in an exact + stop mode (G61.1) manner. In the future, if continuous mode (G64) is desired, the math here + is exactly the same. Instead of motioning all the way to junction point, the machine will + just follow the arc circle defined here. The Arduino doesn't have the CPU cycles to perform + a continuous mode path, but ARM-based microcontrollers most certainly do. + */ + // TODO: Acceleration need to be limited by the minimum of the two junctions. + // TODO: Need to setup a method to handle zero junction speeds when starting from rest. + if (block_buffer_head == block_buffer_tail) { + block->max_entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED; + } else { + // NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta). + float sin_theta_d2 = sqrt(0.5*(1.0-junction_cos_theta)); // Trig half angle identity. Always positive. + block->max_entry_speed_sqr = (block->acceleration * settings.junction_deviation * sin_theta_d2)/(1.0-sin_theta_d2); + } + + // Store block nominal speed and rate + block->nominal_speed_sqr = feed_rate*feed_rate; // (mm/min)^2. Always > 0 + block->nominal_rate = ceil(feed_rate*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + + // Compute and store acceleration and distance traveled per step event. + block->rate_delta = ceil(block->acceleration* + ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) + block->d_next = ceil((block->millimeters*INV_TIME_MULTIPLIER)/block->step_event_count); // (mult*mm/step) + + // Update previous path unit_vector and nominal speed (squared) + memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // pl.previous_unit_vec[] = unit_vec[] + pl.previous_nominal_speed_sqr = block->nominal_speed_sqr; + + // Update planner position + memcpy(pl.position, target_steps, sizeof(target_steps)); // pl.position[] = target_steps[] + memcpy(pl.last_target, target, sizeof(target)); // pl.last_target[] = target[] + + planner_recalculate(); + + // Update buffer head and next buffer head indices. + // NOTE: The buffer head update is atomic since it's one byte. Performed after the new plan + // calculations to help prevent overwriting scenarios with adding a new block to a low buffer. + block_buffer_head = next_buffer_head; + next_buffer_head = next_block_index(block_buffer_head); +} + + +// Reset the planner position vectors. Called by the system abort/initialization routine. +void plan_sync_position() +{ + uint8_t idx; + for (idx=0; idxmillimeters = (block->millimeters*step_events_remaining)/block->step_event_count; + block->step_event_count = step_events_remaining; + + // Re-plan from a complete stop. Reset planner entry speeds and buffer planned pointer. + block->entry_speed_sqr = 0.0; + block->max_entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED; + block_buffer_planned = block_buffer_tail; + planner_recalculate(); +} diff --git a/planner_old.h b/planner_old.h new file mode 100644 index 0000000..84ecc4b --- /dev/null +++ b/planner_old.h @@ -0,0 +1,83 @@ +/* + planner.h - buffers movement commands and manages the acceleration profile plan + Part of Grbl + + Copyright (c) 2011-2013 Sungeun K. Jeon + Copyright (c) 2009-2011 Simen Svale Skogsrud + + 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 + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#ifndef planner_h +#define planner_h +#include "nuts_bolts.h" + +// The number of linear motions that can be in the plan at any give time +#ifndef BLOCK_BUFFER_SIZE + #define BLOCK_BUFFER_SIZE 17 +#endif + +// This struct is used when buffering the setup for each linear movement "nominal" values are as specified in +// the source g-code and may never actually be reached if acceleration management is active. +typedef struct { + + // Fields used by the bresenham algorithm for tracing the line + uint8_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) + uint32_t steps[N_AXIS]; // Step count along each axis + int32_t step_event_count; // The number of step events required to complete this block + + // Fields used by the motion planner to manage acceleration + float nominal_speed_sqr; // Axis-limit adjusted nominal speed for this block in (mm/min)^2 + float entry_speed_sqr; // Entry speed at previous-current block junction in (mm/min)^2 + float max_entry_speed_sqr; // Maximum allowable junction entry speed in (mm/min)^2 + float millimeters; // The total travel of this block in mm + float acceleration; // Axes-limit adjusted line acceleration in mm/min^2 + + // Settings for the trapezoid generator + uint32_t initial_rate; // The step rate at start of block + int32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive) + uint32_t decelerate_after; // The index of the step event on which to start decelerating + uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute + uint32_t d_next; // Scaled distance to next step + +} block_t; + +// Initialize the motion plan subsystem +void plan_init(); + +// Add a new linear movement to the buffer. target[N_AXIS] is the signed, absolute target position +// in millimeters. 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(float *target, float 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. +void plan_discard_current_block(); + +// Gets the current block. Returns NULL if buffer empty +block_t *plan_get_current_block(); + +// Reset the planner position vector (in steps) +void plan_sync_position(); + +// Reinitialize plan with a partially completed block +void plan_cycle_reinitialize(int32_t step_events_remaining); + +// Returns the status of the block ring buffer. True, if buffer is full. +uint8_t plan_check_full_buffer(); + +// Block until all buffered steps are executed +void plan_synchronize(); + +#endif diff --git a/print.c b/print.c index 2f820d5..7a7686b 100644 --- a/print.c +++ b/print.c @@ -77,7 +77,7 @@ void print_uint8_base2(uint8_t n) serial_write('0' + buf[i - 1]); } -static void print_uint32_base10(unsigned long n) +void print_uint32_base10(unsigned long n) { unsigned char buf[10]; uint8_t i = 0; diff --git a/print.h b/print.h index 9983aee..8a161c1 100644 --- a/print.h +++ b/print.h @@ -31,6 +31,8 @@ void printPgmString(const char *s); void printInteger(long n); +void print_uint32_base10(uint32_t n); + void print_uint8_base2(uint8_t n); void printFloat(float n); diff --git a/protocol.c b/protocol.c index a75a141..253ec1f 100644 --- a/protocol.c +++ b/protocol.c @@ -104,6 +104,7 @@ ISR(PINOUT_INT_vect) // limit switches, or the main program. void protocol_execute_runtime() { + st_prep_buffer(); if (sys.execute) { // Enter only if any bit flag is true uint8_t rt_exec = sys.execute; // Avoid calling volatile multiple times diff --git a/report.c b/report.c index 8c90a17..e71ac54 100644 --- a/report.c +++ b/report.c @@ -157,9 +157,9 @@ void report_grbl_settings() { printPgmString(PSTR(" (z v_max, mm/min)\r\n$6=")); printFloat(settings.acceleration[X_AXIS]/(60*60)); // Convert from mm/min^2 for human readability printPgmString(PSTR(" (x accel, mm/sec^2)\r\n$7=")); printFloat(settings.acceleration[Y_AXIS]/(60*60)); // Convert from mm/min^2 for human readability printPgmString(PSTR(" (y accel, mm/sec^2)\r\n$8=")); printFloat(settings.acceleration[Z_AXIS]/(60*60)); // Convert from mm/min^2 for human readability - printPgmString(PSTR(" (z accel, mm/sec^2)\r\n$9=")); printFloat(settings.max_travel[X_AXIS]); - printPgmString(PSTR(" (x max travel, mm)\r\n$10=")); printFloat(settings.max_travel[Y_AXIS]); - printPgmString(PSTR(" (y max travel, mm)\r\n$11=")); printFloat(settings.max_travel[Z_AXIS]); + printPgmString(PSTR(" (z accel, mm/sec^2)\r\n$9=")); printFloat(-settings.max_travel[X_AXIS]); // Grbl internally store this as negative. + printPgmString(PSTR(" (x max travel, mm)\r\n$10=")); printFloat(-settings.max_travel[Y_AXIS]); // Grbl internally store this as negative. + printPgmString(PSTR(" (y max travel, mm)\r\n$11=")); printFloat(-settings.max_travel[Z_AXIS]); // Grbl internally store this as negative. printPgmString(PSTR(" (z max travel, mm)\r\n$12=")); printInteger(settings.pulse_microseconds); printPgmString(PSTR(" (step pulse, usec)\r\n$13=")); printFloat(settings.default_feed_rate); printPgmString(PSTR(" (default feed, mm/min)\r\n$14=")); printInteger(settings.invert_mask); diff --git a/settings.c b/settings.c index 9225a91..8f628a5 100644 --- a/settings.c +++ b/settings.c @@ -169,9 +169,9 @@ uint8_t settings_store_global_setting(int parameter, float value) { case 6: settings.acceleration[X_AXIS] = value*60*60; break; // Convert to mm/min^2 for grbl internal use. case 7: settings.acceleration[Y_AXIS] = value*60*60; break; // Convert to mm/min^2 for grbl internal use. case 8: settings.acceleration[Z_AXIS] = value*60*60; break; // Convert to mm/min^2 for grbl internal use. - case 9: settings.max_travel[X_AXIS] = value; break; - case 10: settings.max_travel[Y_AXIS] = value; break; - case 11: settings.max_travel[Z_AXIS] = value; break; + case 9: settings.max_travel[X_AXIS] = -value; break; // Store as negative for grbl internal use. + case 10: settings.max_travel[Y_AXIS] = -value; break; // Store as negative for grbl internal use. + case 11: settings.max_travel[Z_AXIS] = -value; break; // Store as negative for grbl internal use. case 12: if (value < 3) { return(STATUS_SETTING_STEP_PULSE_MIN); } settings.pulse_microseconds = round(value); break; @@ -206,7 +206,10 @@ uint8_t settings_store_global_setting(int parameter, float value) { break; case 24: if (value) { settings.flags |= BITFLAG_HOMING_ENABLE; } - else { settings.flags &= ~BITFLAG_HOMING_ENABLE; } + else { + settings.flags &= ~BITFLAG_HOMING_ENABLE; + settings.flags &= ~BITFLAG_SOFT_LIMIT_ENABLE; + } break; case 25: settings.homing_dir_mask = trunc(value); break; case 26: settings.homing_feed_rate = value; break; diff --git a/stepper.c b/stepper.c index acb3282..05ddd9e 100644 --- a/stepper.c +++ b/stepper.c @@ -19,20 +19,30 @@ along with Grbl. If not, see . */ -/* The timer calculations of this module informed by the 'RepRap cartesian firmware' by Zack Smith - and Philipp Tiefenbacher. */ - #include #include "stepper.h" #include "config.h" #include "settings.h" #include "planner.h" +#include "nuts_bolts.h" // Some useful constants #define TICKS_PER_MICROSECOND (F_CPU/1000000) -#define CRUISE_RAMP 0 -#define ACCEL_RAMP 1 -#define DECEL_RAMP 2 + +#define RAMP_NOOP_CRUISE 0 +#define RAMP_ACCEL 1 +#define RAMP_DECEL 2 + +#define LOAD_NOOP 0 +#define LOAD_LINE 1 +#define LOAD_BLOCK 2 + +#define ST_NOOP 0 +#define ST_END_OF_BLOCK 1 +#define ST_DECEL 2 +#define ST_DECEL_EOB 3 + +#define SEGMENT_BUFFER_SIZE 10 // Stepper state variable. Contains running data and trapezoid variables. typedef struct { @@ -40,44 +50,92 @@ typedef struct { int32_t counter_x, // Counter variables for the bresenham line tracer counter_y, counter_z; - uint32_t event_count; // Total event count. Retained for feed holds. - uint32_t step_events_remaining; // Steps remaining in motion + uint8_t segment_steps_remaining; // Steps remaining in line motion - // Used by Pramod Ranade inverse time algorithm - int32_t delta_d; // Ranade distance traveled per interrupt tick - int32_t d_counter; // Ranade distance traveled since last step event - uint8_t ramp_count; // Acceleration interrupt tick counter. - uint8_t ramp_type; // Ramp type variable. - uint8_t execute_step; // Flags step execution for each interrupt. + // Used by inverse time algorithm to track step rate + int32_t counter_d; // Inverse time distance traveled since last step event + uint32_t delta_d; // Inverse time distance traveled per interrupt tick + uint32_t d_per_tick; + // Used by the stepper driver interrupt + uint8_t execute_step; // Flags step execution for each interrupt. + uint8_t step_pulse_time; // Step pulse reset time after step rise + uint8_t out_bits; // The next stepping-bits to be output + uint8_t load_flag; + + uint8_t ramp_count; + uint8_t ramp_type; } stepper_t; static stepper_t st; -static block_t *current_block; // A pointer to the block currently being traced -// Used by the stepper driver interrupt -static uint8_t step_pulse_time; // Step pulse reset time after step rise -static uint8_t out_bits; // The next stepping-bits to be output +// Stores stepper buffer common data. Can change planner mid-block in special conditions. +typedef struct { + int32_t step_events_remaining; // Tracks step event count for the executing planner block + uint32_t d_next; // Scaled distance to next step + uint32_t initial_rate; // Initialized step rate at re/start of a planner block + uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute + uint32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive) + int32_t decelerate_after; + float mm_per_step; +} st_data_t; +static st_data_t segment_data[SEGMENT_BUFFER_SIZE]; -// NOTE: If the main interrupt is guaranteed to be complete before the next interrupt, then -// this blocking variable is no longer needed. Only here for safety reasons. -static volatile uint8_t busy; // True when "Stepper Driver Interrupt" is being serviced. Used to avoid retriggering that handler. +// Primary stepper motion buffer +typedef struct { + uint8_t n_step; + uint8_t st_data_index; + uint8_t flag; +} st_segment_t; +static st_segment_t segment_buffer[SEGMENT_BUFFER_SIZE]; -// __________________________ -// /| |\ _________________ ^ -// / | | \ /| |\ | -// / | | \ / | | \ s -// / | | | | | \ p -// / | | | | | \ e -// +-----+------------------------+---+--+---------------+----+ e -// | BLOCK 1 | BLOCK 2 | d -// -// time -----> -// -// The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta -// until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after -// after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as -// +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. +static volatile uint8_t segment_buffer_tail; +static uint8_t segment_buffer_head; +static uint8_t segment_next_head; +static volatile uint8_t busy; // Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though. +static plan_block_t *pl_current_block; // A pointer to the planner block currently being traced +static st_segment_t *st_current_segment; +static st_data_t *st_current_data; + +static plan_block_t *pl_prep_block; // A pointer to the planner block being prepped into the stepper buffer +static uint8_t pl_prep_index; +static st_data_t *st_prep_data; +static uint8_t st_data_prep_index; + + +// Returns the index of the next block in the ring buffer +// NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication. +static uint8_t next_block_index(uint8_t block_index) +{ + block_index++; + if (block_index == SEGMENT_BUFFER_SIZE) { block_index = 0; } + return(block_index); +} + +static uint8_t next_block_pl_index(uint8_t block_index) +{ + block_index++; + if (block_index == 18) { block_index = 0; } + return(block_index); +} + + +/* __________________________ + /| |\ _________________ ^ + / | | \ /| |\ | + / | | \ / | | \ s + / | | | | | \ p + / | | | | | \ e + +-----+------------------------+---+--+---------------+----+ e + | BLOCK 1 | BLOCK 2 | d + + time -----> + + The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta + until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after + after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as + +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. +*/ // Stepper state initialization. Cycle should only start if the st.cycle_start flag is // enabled. Startup init and limits call this function but shouldn't start the cycle. @@ -91,14 +149,17 @@ void st_wake_up() } if (sys.state == STATE_CYCLE) { // Initialize stepper output bits - out_bits = settings.invert_mask; + st.out_bits = settings.invert_mask; // Initialize step pulse timing from settings. - step_pulse_time = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3); + st.step_pulse_time = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3); // Enable stepper driver interrupt st.execute_step = false; + st.load_flag = LOAD_BLOCK; + TCNT2 = 0; // Clear Timer2 TIMSK2 |= (1<direction_bits ^ settings.invert_mask; - st.execute_step = true; // Set flag to set direction bits. + // NOTE: Loads after a step event. At high rates above 1/2 ISR frequency, there is + // a small chance that this will load at the same time as a step event. Hopefully, + // the overhead for this loading event isn't too much.. possibly 2-5 usec. + + // NOTE: The stepper algorithm must control the planner buffer tail as it completes + // the block moves. Otherwise, a feed hold can leave a few step buffer line moves + // without the correct planner block information. - // Initialize Bresenham variables - st.counter_x = (current_block->step_event_count >> 1); - st.counter_y = st.counter_x; - st.counter_z = st.counter_x; - st.event_count = current_block->step_event_count; - st.step_events_remaining = st.event_count; + st_current_segment = &segment_buffer[segment_buffer_tail]; - // During feed hold, do not update Ranade counter, rate, or ramp type. Keep decelerating. - if (sys.state == STATE_CYCLE) { - // Initialize Ranade variables - st.d_counter = current_block->d_next; - st.delta_d = current_block->initial_rate; - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; + // Load number of steps to execute from stepper buffer + st.segment_steps_remaining = st_current_segment->n_step; + + // Check if the counters need to be reset for a new planner block + if (st.load_flag == LOAD_BLOCK) { + pl_current_block = plan_get_current_block(); // Should always be there. Stepper buffer handles this. + st_current_data = &segment_data[segment_buffer[segment_buffer_tail].st_data_index]; //st_current_segment->st_data_index]; - // Initialize ramp type. - if (st.step_events_remaining == current_block->decelerate_after) { st.ramp_type = DECEL_RAMP; } - else if (st.delta_d == current_block->nominal_rate) { st.ramp_type = CRUISE_RAMP; } - else { st.ramp_type = ACCEL_RAMP; } + // Initialize direction bits for block + st.out_bits = pl_current_block->direction_bits ^ settings.invert_mask; + st.execute_step = true; // Set flag to set direction bits upon next ISR tick. + + // Initialize Bresenham line counters + st.counter_x = (pl_current_block->step_event_count >> 1); + st.counter_y = st.counter_x; + st.counter_z = st.counter_x; + + // Initialize inverse time and step rate counter data + st.counter_d = st_current_data->d_next; // d_next always greater than delta_d. + + // During feed hold, do not update rate or ramp type. Keep decelerating. +// if (sys.state == STATE_CYCLE) { + st.delta_d = st_current_data->initial_rate; +// if (st.delta_d == st_current_data->nominal_rate) { +// st.ramp_type = RAMP_NOOP_CRUISE; + st.ramp_type = RAMP_ACCEL; + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid +// } +// } + if (st.delta_d < MINIMUM_STEP_RATE) { st.d_per_tick = MINIMUM_STEP_RATE; } + else { st.d_per_tick = st.delta_d; } + + } + + // Acceleration and cruise handled by ramping. Just check for deceleration. + if (st_current_segment->flag == ST_DECEL || st_current_segment->flag == ST_DECEL_EOB) { + if (st.ramp_type == RAMP_NOOP_CRUISE) { + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid + } else { + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; // Set ramp counter for triangle + } + st.ramp_type = RAMP_DECEL; } + st.load_flag = LOAD_NOOP; // Motion loaded. Set no-operation flag until complete. + } else { + // Can't discard planner block here if a feed hold stops in middle of block. st_go_idle(); bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end return; // Nothing to do but exit. } + } // Adjust inverse time counter for ac/de-celerations if (st.ramp_type) { - // Tick acceleration ramp counter - st.ramp_count--; - if (st.ramp_count == 0) { + st.ramp_count--; // Tick acceleration ramp counter + if (st.ramp_count == 0) { // Adjust step rate when its time st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter - if (st.ramp_type == ACCEL_RAMP) { // Adjust velocity for acceleration - st.delta_d += current_block->rate_delta; - if (st.delta_d >= current_block->nominal_rate) { // Reached cruise state. - st.ramp_type = CRUISE_RAMP; - st.delta_d = current_block->nominal_rate; // Set cruise velocity + if (st.ramp_type == RAMP_ACCEL) { // Adjust velocity for acceleration + st.delta_d += st_current_data->rate_delta; + if (st.delta_d >= st_current_data->nominal_rate) { // Reached nominal rate. + st.delta_d = st_current_data->nominal_rate; // Set cruising velocity + st.ramp_type = RAMP_NOOP_CRUISE; // Set ramp flag to ignore } - } else if (st.ramp_type == DECEL_RAMP) { // Adjust velocity for deceleration - if (st.delta_d > current_block->rate_delta) { - st.delta_d -= current_block->rate_delta; + } else { // Adjust velocity for deceleration + if (st.delta_d > st_current_data->rate_delta) { + st.delta_d -= st_current_data->rate_delta; } else { + + // Moving near zero feed rate. Gracefully slow down. st.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow. + + // Check for and handle feed hold exit? At this point, machine is stopped. + } } + // Finalize adjusted step rate. Ensure minimum. + if (st.delta_d < MINIMUM_STEP_RATE) { st.d_per_tick = MINIMUM_STEP_RATE; } + else { st.d_per_tick = st.delta_d; } } } - - // Iterate Pramod Ranade inverse time counter. Triggers each Bresenham step event. - if (st.delta_d < MINIMUM_STEP_RATE) { st.d_counter -= MINIMUM_STEP_RATE; } - else { st.d_counter -= st.delta_d; } + + // Iterate inverse time counter. Triggers each Bresenham step event. + st.counter_d -= st.d_per_tick; // Execute Bresenham step event, when it's time to do so. - if (st.d_counter < 0) { - st.d_counter += current_block->d_next; - - // Check for feed hold state and execute accordingly. - if (sys.state == STATE_HOLD) { - if (st.ramp_type != DECEL_RAMP) { - st.ramp_type = DECEL_RAMP; - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; - } - if (st.delta_d <= current_block->rate_delta) { - st_go_idle(); - bit_true(sys.execute,EXEC_CYCLE_STOP); - return; - } - } - - // TODO: Vary Bresenham resolution for smoother motions or enable faster step rates (>20kHz). - - out_bits = current_block->direction_bits; // Reset out_bits and reload direction bits + if (st.counter_d < 0) { + st.counter_d += st_current_data->d_next; // Reload inverse time counter + + st.out_bits = pl_current_block->direction_bits; // Reset out_bits and reload direction bits st.execute_step = true; // Execute step displacement profile by Bresenham line algorithm - st.counter_x -= current_block->steps_x; + st.counter_x -= pl_current_block->steps[X_AXIS]; if (st.counter_x < 0) { - out_bits |= (1<step_event_count; + if (st.out_bits & (1<steps_y; + st.counter_y -= pl_current_block->steps[Y_AXIS]; if (st.counter_y < 0) { - out_bits |= (1<step_event_count; + if (st.out_bits & (1<steps_z; + st.counter_z -= pl_current_block->steps[Z_AXIS]; if (st.counter_z < 0) { - out_bits |= (1<step_event_count; + if (st.out_bits & (1<decelerate_after) { - st.ramp_type = DECEL_RAMP; - if (st.step_events_remaining == current_block->decelerate_after) { - if (st.delta_d == current_block->nominal_rate) { - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid - } else { - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; // Set ramp counter for triangle - } - } - } + st.segment_steps_remaining--; // Decrement step events count + if (st.segment_steps_remaining == 0) { + + // Line move is complete, set load line flag to check for new move. + // Check if last line move in planner block. Discard if so. + if (st_current_segment->flag == ST_END_OF_BLOCK || st_current_segment->flag == ST_DECEL_EOB) { + plan_discard_current_block(); + st.load_flag = LOAD_BLOCK; + } else { + st.load_flag = LOAD_LINE; } - } else { - // If current block is finished, reset pointer - current_block = NULL; - plan_discard_current_block(); + + // Discard current segment + segment_buffer_tail = next_block_index( segment_buffer_tail ); + + // NOTE: sys.position updates could be done here. The bresenham counters can have + // their own fast 8-bit addition-only counters. Here we would check the direction and + // apply it to sys.position accordingly. However, this could take too much time. + } - out_bits ^= settings.invert_mask; // Apply step port invert mask + st.out_bits ^= settings.invert_mask; // Apply step port invert mask } busy = false; // SPINDLE_ENABLE_PORT ^= 1<step_events_remaining); +// st.ramp_type = RAMP_ACCEL; +// st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; +// st.delta_d = 0; +// sys.state = STATE_QUEUED; +// } else { +// sys.state = STATE_IDLE; +// } sys.state = STATE_IDLE; - } + } + + +/* Preps stepper buffer. Called from main program. + + NOTE: There doesn't seem to be a great way to figure out how many steps occur within + a set number of ISR ticks. Numerical round-off and CPU overhead always seems to be a + critical problem. So, either numerical round-off checks could be made to account for + them, while CPU overhead could be minimized in some way, or we can flip the algorithm + around to have the stepper algorithm track number of steps over an indeterminant amount + of time instead. + In other words, we use the planner velocity floating point data to get an estimate of + the number of steps we want to execute. We then back out the approximate velocity for + the planner to use, which should be much more robust to round-off error. The main problem + now is that we are loading the stepper algorithm to handle acceleration now, rather than + pre-calculating with the main program. This approach does make sense in the way that + planner velocities and stepper profiles can be traced more accurately. + Which is better? Very hard to tell. The time-based algorithm would be able to handle + Bresenham step adaptive-resolution much easier and cleaner. Whereas, the step-based would + require some additional math in the stepper algorithm to adjust on the fly, plus adaptation + would occur in a non-deterministic manner. + I suppose it wouldn't hurt to build both to see what's better. Just a lot more work. + + TODO: Need to describe the importance of continuations of step pulses between ramp states + and planner blocks. This has to do with Alden's problem with step "phase". The things I've + been doing here limit this phase issue by truncating some of the ramp timing for certain + events like deceleration initialization and end of block. +*/ +void st_prep_buffer() +{ + while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer. + + // Determine if we need to load a new planner block. + if (pl_prep_block == NULL) { + pl_prep_block = plan_get_block_by_index(pl_prep_index); + if (pl_prep_block == NULL) { return; } // No more planner blocks. Let stepper finish out. + + // Prepare commonly shared planner block data for the ensuing step buffer moves + st_data_prep_index = next_block_index(st_data_prep_index); + st_prep_data = &segment_data[st_data_prep_index]; + + // Initialize Bresenham variables + st_prep_data->step_events_remaining = pl_prep_block->step_event_count; + + // Convert new block to stepper variables. + // NOTE: This data can change mid-block from normal planner updates and feedrate overrides. Must + // be maintained as these execute. + // TODO: If the planner updates this block, particularly from a deceleration to an acceleration, + // we must reload the initial rate data, such that the velocity profile is re-constructed correctly. + st_prep_data->initial_rate = ceil(sqrt(pl_prep_block->entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + + // This data doesn't change. Could be performed in the planner, but fits nicely here. + // Although, acceleration can change for S-curves. So keep it here. + st_prep_data->rate_delta = ceil(pl_prep_block->acceleration* + ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) + // This definitely doesn't change, but could be precalculated in a way to help some of the + // math in this handler, i.e. millimeters per step event data. + st_prep_data->d_next = ceil((pl_prep_block->millimeters*INV_TIME_MULTIPLIER)/pl_prep_block->step_event_count); // (mult*mm/step) + st_prep_data->mm_per_step = pl_prep_block->millimeters/pl_prep_block->step_event_count; + + // Calculate trapezoid data from planner. + st_prep_data->decelerate_after = calculate_trapezoid_for_block(pl_prep_index); + + } + + + /* + TODO: Need to check for a planner flag to indicate a change to this planner block. + If so, need to check for a change in acceleration state, from deceleration to acceleration, + to reset the stepper ramp counters and the initial_rate data to trace the new + ac/de-celeration profile correctly. + No change conditions: + - From nominal speed to acceleration from feedrate override + - From nominal speed to new deceleration. + - From acceleration to new deceleration point later or cruising point. + - From acceleration to immediate deceleration? Can happen during feedrate override + and slowing down, but likely ok by enforcing the normal ramp counter protocol. + Change conditions: + - From deceleration to acceleration, i.e. common with jogging when new blocks are added. + */ + + st_segment_t *st_prep_segment = &segment_buffer[segment_buffer_head]; + st_prep_segment->st_data_index = st_data_prep_index; + + // TODO: How do you cheaply compute n_step without a sqrt()? Could be performed as 'bins'. + st_prep_segment->n_step = 250; //floor( (exit_speed*approx_time)/mm_per_step ); +// st_segment->n_step = max(st_segment->n_step,MINIMUM_STEPS_PER_BLOCK); // Ensure it moves for very slow motions? +// st_segment->n_step = min(st_segment->n_step,MAXIMUM_STEPS_PER_BLOCK); // Prevent unsigned int8 overflow. + + // Check if n_step exceeds steps remaining in planner block. If so, truncate. + if (st_prep_segment->n_step > st_prep_data->step_events_remaining) { + st_prep_segment->n_step = st_prep_data->step_events_remaining; + } + + // Check if n_step exceeds decelerate point in block. Need to perform this so that the + // ramp counters are reset correctly in the stepper algorithm. Can be 1 step, but should + // be OK since it is likely moving at a fast rate already. + if (st_prep_data->decelerate_after > 0) { + if (st_prep_segment->n_step > st_prep_data->decelerate_after) { + st_prep_segment->n_step = st_prep_data->decelerate_after; + } + } + +// float distance, exit_speed_sqr; +// distance = st_prep_segment->n_step*st_prep_data->mm_per_step; // Always greater than zero +// if (st_prep_data->step_events_remaining >= pl_prep_block->decelerate_after) { +// exit_speed_sqr = pl_prep_block->entry_speed_sqr - 2*pl_prep_block->acceleration*distance; +// } else { // Acceleration or cruising ramp +// if (pl_prep_block->entry_speed_sqr < pl_prep_block->nominal_speed_sqr) { +// exit_speed_sqr = pl_prep_block->entry_speed_sqr + 2*pl_prep_block->acceleration*distance; +// if (exit_speed_sqr > pl_prep_block->nominal_speed_sqr) { exit_speed_sqr = pl_prep_block->nominal_speed_sqr; } +// } else { +// exit_speed_sqr = pl_prep_block->nominal_speed_sqr; +// } +// } + + // Update planner block variables. +// pl_prep_block->entry_speed_sqr = max(0.0,exit_speed_sqr); +// pl_prep_block->max_entry_speed_sqr = exit_speed_sqr; // ??? Overwrites the corner speed. May need separate variable. +// pl_prep_block->millimeters -= distance; // Potential round-off error near end of block. +// pl_prep_block->millimeters = max(0.0,pl_prep_block->millimeters); // Shouldn't matter. + + // Update stepper block variables. + st_prep_data->step_events_remaining -= st_prep_segment->n_step; + if ( st_prep_data->step_events_remaining == 0 ) { + // Move planner pointer to next block + if (st_prep_data->decelerate_after == 0) { + st_prep_segment->flag = ST_DECEL_EOB; + } else { + st_prep_segment->flag = ST_END_OF_BLOCK; + } + pl_prep_index = next_block_pl_index(pl_prep_index); + pl_prep_block = NULL; +printString("EOB"); + } else { + if (st_prep_data->decelerate_after == 0) { + st_prep_segment->flag = ST_DECEL; + } else { + st_prep_segment->flag = ST_NOOP; + } +printString("x"); + } + st_prep_data->decelerate_after -= st_prep_segment->n_step; + + // New step block completed. Increment step buffer indices. + segment_buffer_head = segment_next_head; + segment_next_head = next_block_index(segment_buffer_head); + +printInteger((long)st_prep_segment->n_step); +printString(" "); +printInteger((long)st_prep_data->decelerate_after); +printString(" "); +printInteger((long)st_prep_data->step_events_remaining); + } +} diff --git a/stepper.h b/stepper.h index 0cb4189..74ab717 100644 --- a/stepper.h +++ b/stepper.h @@ -45,4 +45,6 @@ void st_cycle_reinitialize(); // Initiates a feed hold of the running program void st_feed_hold(); +void st_prep_buffer(); + #endif diff --git a/stepper_new2.c b/stepper_new2.c new file mode 100644 index 0000000..c248d4d --- /dev/null +++ b/stepper_new2.c @@ -0,0 +1,601 @@ +/* + stepper.c - stepper motor driver: executes motion plans using stepper motors + Part of Grbl + + Copyright (c) 2011-2013 Sungeun K. Jeon + Copyright (c) 2009-2011 Simen Svale Skogsrud + + 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 + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#include +#include "stepper.h" +#include "config.h" +#include "settings.h" +#include "planner.h" +#include "nuts_bolts.h" + +// Some useful constants +#define TICKS_PER_MICROSECOND (F_CPU/1000000) +#define CRUISE_RAMP 0 +#define ACCEL_RAMP 1 +#define DECEL_RAMP 2 + +#define LOAD_NOOP 0 +#define LOAD_LINE 1 +#define LOAD_BLOCK 2 + +// Stepper state variable. Contains running data and trapezoid variables. +typedef struct { + // Used by the bresenham line algorithm + int32_t counter_x, // Counter variables for the bresenham line tracer + counter_y, + counter_z; + int32_t step_events_remaining; // Steps remaining in line motion + + // Used by inverse time algorithm + int32_t delta_d; // Inverse time distance traveled per interrupt tick + int32_t d_counter; // Inverse time distance traveled since last step event + int32_t d_per_tick; + + // Used by the stepper driver interrupt + uint8_t execute_step; // Flags step execution for each interrupt. + uint8_t step_pulse_time; // Step pulse reset time after step rise + uint8_t out_bits; // The next stepping-bits to be output + uint8_t load_flag; +} stepper_t; +static stepper_t st; + +#define STEPPER_BUFFER_SIZE 5 +typedef struct { + int32_t event_count; + int32_t rate; + uint8_t end_of_block; + uint8_t tick_count; + + int32_t initial_rate; // The step rate at start of block + int32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive) + int32_t decelerate_after; // The index of the step event on which to start decelerating + int32_t nominal_rate; // The nominal step rate for this block in step_events/minute + int32_t d_next; // Scaled distance to next step + +} stepper_buffer_t; +static stepper_buffer_t step_buffer[STEPPER_BUFFER_SIZE]; + +static volatile uint8_t step_buffer_tail; +static uint8_t step_buffer_head; +static uint8_t step_next_head; + +// NOTE: If the main interrupt is guaranteed to be complete before the next interrupt, then +// this blocking variable is no longer needed. Only here for safety reasons. +static volatile uint8_t busy; // True when "Stepper Driver Interrupt" is being serviced. Used to avoid retriggering that handler. +static plan_block_t *plan_current_block; // A pointer to the planner block currently being traced + + +/* __________________________ + /| |\ _________________ ^ + / | | \ /| |\ | + / | | \ / | | \ s + / | | | | | \ p + / | | | | | \ e + +-----+------------------------+---+--+---------------+----+ e + | BLOCK 1 | BLOCK 2 | d + + time -----> + + The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta + until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after + after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as + +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. +*/ + +// Stepper state initialization. Cycle should only start if the st.cycle_start flag is +// enabled. Startup init and limits call this function but shouldn't start the cycle. +void st_wake_up() +{ + // Enable steppers by resetting the stepper disable port + if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { + STEPPERS_DISABLE_PORT |= (1<> 3); + // Enable stepper driver interrupt + st.execute_step = false; + TCNT2 = 0; // Clear Timer2 + TIMSK2 |= (1<direction_bits ^ settings.invert_mask; + st.execute_step = true; // Set flag to set direction bits. + + st.counter_x = (plan_current_block->step_event_count >> 1); + st.counter_y = st.counter_x; + st.counter_z = st.counter_x; + + // This is correct. Sets the total time before the next step occurs. + st.counter_d = plan_current_block->d_next; // d_next always greater than delta_d. + + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid + + } + + st.load_flag = LOAD_NOOP; // Line motion loaded. Set no-operation flag until complete. + + } else { + // Can't discard planner block here if a feed hold stops in middle of block. + st_go_idle(); + bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end + return; // Nothing to do but exit. + } + + } + + // Iterate inverse time counter. Triggers each Bresenham step event. + st.counter_d -= st.delta_d; + + // Execute Bresenham step event, when it's time to do so. + if (st.counter_d < 0) { + st.counter_d += plan_current_block->d_next; // Reload inverse time counter + + st.out_bits = plan_current_block->direction_bits; // Reset out_bits and reload direction bits + st.execute_step = true; + + // Execute step displacement profile by Bresenham line algorithm + st.counter_x -= plan_current_block->steps[X_AXIS]; + if (st.counter_x < 0) { + st.out_bits |= (1<step_event_count; + if (st.out_bits & (1<steps[Y_AXIS]; + if (st.counter_y < 0) { + st.out_bits |= (1<step_event_count; + if (st.out_bits & (1<steps[Z_AXIS]; + if (st.counter_z < 0) { + st.out_bits |= (1<step_event_count; + if (st.out_bits & (1<step_event_count; + //st.step_events_remaining = st.event_count; + + // Convert new block to stepper variables. + // NOTE: This data can change mid-block from normal planner updates and feedrate overrides. Must + // be maintained as these execute. + // TODO: The initial rate needs to be sent back to the planner to update the entry speed + block->initial_rate = ceil(sqrt(plan_current_block->entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + block->nominal_rate = ceil(plan_current_block->nominal_speed*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + + // This data doesn't change. Could be performed in the planner, but fits nicely here. + // Although, acceleration can change for S-curves. So keep it here. + block->rate_delta = ceil(plan_current_block->acceleration* + ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) + // This definitely doesn't change, but could be precalculated in a way to help some of the + // math in this handler, i.e. millimeters per step event data. + block->d_next = ceil((plan_current_block->millimeters*INV_TIME_MULTIPLIER)/plan_current_block->step_event_count); // (mult*mm/step) + + // During feed hold, do not update Ranade counter, rate, or ramp type. Keep decelerating. + if (sys.state == STATE_CYCLE) { } + } + + +// Track instead the trapezoid line and use the average of the entry and exit velocities +// to determine step rate. This should take care of the deceleration issue automatically... +// i think. +// First need to figure out what type of profile is segment is, i.e. acceleration only, accel +// to decel triangle, cruise to decel, or all three. May need more profile data to compute this +// from the planner itself, like accelerate until. +// Another issue. This is only tracking the velocity profile, not the distance covered over that +// time period. This can lead to an unsynchronized velocity profile and steps executed. But, +// how much drift is there really? Enough to be a problem? Not sure. I would think typical +// drift would be on the order of a few steps, more depending on step resolution. +entry_rate = last_exit_rate; +time = 250 ISR ticks per acceleration tick. +distance = 0; +if (distance_traveled < accelerate_until) + exit_rate = entry_rate + acceleration*time; + if (exit_rate > nominal_rate) { + exit_rate = nominal_rate; + time = 2*(accelerate_until-distance_travel)/(entry_rate+nominal_rate); + // distance = accelerate_until; // Enforce distance? + // Truncate this segment. + } +} else if (distance_traveled >= decelerate_after) { + if (accelerate_until == decelerate_after) { + time = last time; + exit_rate = entry_rate; + } else { + exit_rate = entry_rate - acceleration*time; + } +} else { + exit_rate = nominal_rate; // Just cruise + distance = nominal_rate*time; + if (distance > decelerate_after) { // Truncate segment at nominal rate. + time = (decelerate_after-distance_traveled)/(nominal_rate); + distance = decelerate_after; + } +} +mean_rate = 0.5*(entry_rate+exit_rate); +distance = mean_rate*time; + + +if (entry_rate < nominal_rate) { + if (entry_distance < decelerate_after) { // Acceleration case + exit_rate = entry_rate + acceleration*time + exit_rate = min(exit_rate,nominal_rate); +mean_rate = 0.5*(entry_rate + exit_rate); +distance = mean_rate*time; +if (distance > decelerate_after) { + exit_rate = + + + // If the MINIMUM_STEP_RATE is less than ACCELERATION_TICKS_PER_SECOND then there can be + // rate adjustements that have less than one step per tick. + // How do you deal with the remainer? +time = 250 ISR ticks per acceleration tick. (30000/120) +delta_d*time // mm per acceleration tick +delta_d*time/d_next // number of steps/acceleration_tick. Chance of integer overflow. +delta_d*time/d_next + last_remainder. // steps/acceleration_tick. +n_step*d_next/delta_d // number of ISR ticks for enforced n_steps. + +// In floating point? Then convert? +// Requires exact millimeters. Roundoff might be a problem. But could be corrected by just +// checking if the total step event counts are performed. +// Could be limited by float conversion and about 1e7 steps per block. +line_mm = feed_rate / acc_tick // mm per acc_tick +n_steps = floor(line_mm * step_event_remaining/millimeters_remaining) // steps. float 7.2 digits|int32 10 digits +millimeters_remaining -= line_mm; +step_events_remaining -= n_steps; + +// There doesn't seem to be a way to avoid this divide here. +line_mm = feed_rate / acc_tick // mm per acc_tick +n_steps = floor( (line_mm+line_remainder) * step_event_count/millimeters) // steps. float 7.2 digits|int32 10 digits +line_remainder = line_mm - n_steps*(millimeters/step_event_count); + +// Need to handle when rate is very very low, i.e. less than one step per accel tick. +// Could be bounded by MINIMUM_STEP_RATE. + +// 1. Figure out how many steps occur exactly within n ISR ticks. +// 2. Account for step-time remainder for next line motion exactly. +// 3. At the end of block, determine exact number of ISR ticks to finish the steps. Or,\ + have the ISR track steps to exit on time. It would require an extra counter. + +// NOTE: There doesn't seem to be a great way to figure out how many steps occur within +// a set number of ISR ticks. Numerical round-off and CPU overhead always seems to be a +// critical problem. So, either numerical round-off checks could be made to account for +// them, while CPU overhead could be minimized in some way, or we can flip the algorithm +// around to have the stepper algorithm track number of steps over an indeterminant amount +// of time instead. +// In other words, we use the planner velocity floating point data to get an estimate of +// the number of steps we want to execute. We then back out the approximate velocity for +// the planner to use, which should be much more robust to round-off error. The main problem +// now is that we are loading the stepper algorithm to handle acceleration now, rather than +// pre-calculating with the main program. This approach does make sense in the way that +// planner velocities and stepper profiles can be traced more accurately. +// Which is better? Very hard to tell. The time-based algorithm would be able to handle +// Bresenham step adaptive-resolution much easier and cleaner. Whereas, the step-based would +// require some additional math in the stepper algorithm to adjust on the fly, plus adaptation +// would occur in a non-deterministic manner. +// I suppose it wouldn't hurt to build both to see what's better. Just a lot more work. + +feed_rate/120 = millimeters per acceleration tick + + + steps? + d_next // (mult*mm/step) + rate // (mult*mm/isr_tic) + rate/d_next // step/isr_tic + + if (plan_current_block->step_events_remaining <= plan_current_block->decelerate_after) { + // Determine line segment velocity and associated inverse time counter. + if (step_block.ramp_type == ACCEL_RAMP) { // Adjust velocity for acceleration + step_block.delta_d += plan_current_block->rate_delta; + if (step_block.delta_d >= plan_current_block->nominal_rate) { // Reached cruise state. + step_block.ramp_type = CRUISE_RAMP; + step_block.delta_d = plan_current_block->nominal_rate; // Set cruise velocity + } + } + } else { // Adjust velocity for deceleration + if (step_block.delta_d > plan_current_block->rate_delta) { + step_block.delta_d -= plan_current_block->rate_delta; + } else { + step_block.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow. + } + } + + // Incorrect. Can't overwrite delta_d. Needs to override instead. + if (step_block.delta_d < MINIMUM_STEP_RATE) { step_block.delta_d = MINIMUM_STEP_RATE; } + + /* - Compute the number of steps needed to complete this move over the move time, i.e. + ISR_TICKS_PER_ACCELERATION_TICK. + - The first block in the buffer is half of the move time due to midpoint rule. + - Check if this reaches the deceleration after location. If so, truncate move. Also, + if this is a triangle move, double the truncated move to stay with midpoint rule. + NOTE: This can create a stepper buffer move down to just one step in length. + - Update the planner block entry speed for the planner to compute from end of the + stepper buffer location. + - If a feed hold occurs, begin to enforce deceleration, while enforcing the above rules. + When the deceleration is complete, all we need to do is update the planner block + entry speed and force a replan. + */ + + // Planner block move completed. + // TODO: planner buffer tail no longer needs to be volatile. only accessed by main program. + if (st.step_events_remaining == 0) { + plan_current_block = NULL; // Set flag that we are done with this planner block. + plan_discard_current_block(); + } + + + + + + step_buffer_head = step_next_head; + step_next_head = next_block_index(step_buffer_head); + + } +} diff --git a/stepper_new.c b/stepper_new_dual_ISR.c similarity index 96% rename from stepper_new.c rename to stepper_new_dual_ISR.c index ae6bd8b..87f87bc 100644 --- a/stepper_new.c +++ b/stepper_new_dual_ISR.c @@ -34,9 +34,7 @@ // Stepper state variable. Contains running data and trapezoid variables. typedef struct { // Used by the bresenham line algorithm - int32_t counter_x, // Counter variables for the bresenham line tracer - counter_y, - counter_z; + int32_t counter[N_AXIS]; // Counter variables for the bresenham line tracer uint32_t event_count; // Total event count. Retained for feed holds. uint32_t step_events_remaining; // Steps remaining in motion @@ -182,26 +180,29 @@ won't take too much time in the interrupt. // Prepare Bresenham step event, when it's time to do so. if (st.d_counter < 0) { st.d_counter += current_block->d_next; - - // Load next step - out_bits = current_block->direction_bits; // Reset out_bits and reload direction bits st.execute_step = true; + + // Configure next step + out_bits = current_block->direction_bits; // Reset out_bits and reload direction bits // Execute step displacement profile by Bresenham line algorithm - st.counter_x -= current_block->steps_x; - if (st.counter_x < 0) { + st.counter[X_AXIS] -= current_block->steps_x; // Doesn't change when set up. + if (st.counter[X_AXIS] < 0) { out_bits |= (1<steps_y; - if (st.counter_y < 0) { + st.counter[Y_AXIS] -= current_block->steps_y; + if (st.counter[Y_AXIS] < 0) { out_bits |= (1<steps_z; - if (st.counter_z < 0) { + st.counter[Z_AXIS] -= current_block->steps_z; + if (st.counter[Z_AXIS] < 0) { out_bits |= (1<. +*/ + +#include +#include "stepper.h" +#include "config.h" +#include "settings.h" +#include "planner.h" +#include "nuts_bolts.h" + +// Some useful constants +#define TICKS_PER_MICROSECOND (F_CPU/1000000) + +#define RAMP_NOOP_CRUISE 0 +#define RAMP_ACCEL 1 +#define RAMP_DECEL 2 + +#define LOAD_NOOP 0 +#define LOAD_LINE 1 +#define LOAD_BLOCK 2 + +#define ST_NOOP 0 +#define ST_END_OF_BLOCK 1 + +// Stepper state variable. Contains running data and trapezoid variables. +typedef struct { + // Used by the bresenham line algorithm + int32_t counter_x, // Counter variables for the bresenham line tracer + counter_y, + counter_z; + int8_t segment_steps_remaining; // Steps remaining in line motion + + // Used by inverse time algorithm to track step rate + int32_t counter_d; // Inverse time distance traveled since last step event + int32_t delta_d; // Inverse time distance traveled per interrupt tick + int32_t d_per_tick; + + // Used by the stepper driver interrupt + uint8_t execute_step; // Flags step execution for each interrupt. + uint8_t step_pulse_time; // Step pulse reset time after step rise + uint8_t out_bits; // The next stepping-bits to be output + uint8_t load_flag; + + uint8_t ramp_count; + uint8_t ramp_type; +} stepper_t; +static stepper_t st; + +#define SEGMENT_BUFFER_SIZE 5 +// Stores stepper buffer common data. Can change planner mid-block in special conditions. +typedef struct { + int32_t step_events_remaining; // Tracks step event count for the executing planner block + int32_t d_next; // Scaled distance to next step + float mm_per_step; +} st_data_t; +static st_data_t segment_data[SEGMENT_BUFFER_SIZE]; + +// Primary stepper motion buffer +typedef struct { + uint8_t n_step; + int32_t rate; + uint8_t st_data_index; + uint8_t flag; +} st_segment_t; +static st_segment_t segment_buffer[SEGMENT_BUFFER_SIZE]; + +static volatile uint8_t segment_buffer_tail; +static uint8_t segment_buffer_head; +static uint8_t segment_next_head; + +static volatile uint8_t busy; // Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though. +static plan_block_t *pl_current_block; // A pointer to the planner block currently being traced +static st_segment_t *st_current_segment; +static st_data_t *st_current_data; + +static plan_block_t *pl_prep_block; // A pointer to the planner block being prepped into the stepper buffer +static uint8_t pl_prep_index; +static st_data_t *st_prep_data; +static uint8_t st_data_prep_index; + + +// Returns the index of the next block in the ring buffer +// NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication. +static uint8_t next_block_index(uint8_t block_index) +{ + block_index++; + if (block_index == SEGMENT_BUFFER_SIZE) { block_index = 0; } + return(block_index); +} + + +/* __________________________ + /| |\ _________________ ^ + / | | \ /| |\ | + / | | \ / | | \ s + / | | | | | \ p + / | | | | | \ e + +-----+------------------------+---+--+---------------+----+ e + | BLOCK 1 | BLOCK 2 | d + + time -----> + + The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta + until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after + after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as + +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. +*/ + +// Stepper state initialization. Cycle should only start if the st.cycle_start flag is +// enabled. Startup init and limits call this function but shouldn't start the cycle. +void st_wake_up() +{ + // Enable steppers by resetting the stepper disable port + if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { + STEPPERS_DISABLE_PORT |= (1<> 3); + // Enable stepper driver interrupt + st.execute_step = false; + TCNT2 = 0; // Clear Timer2 + TIMSK2 |= (1<n_step; + st.delta_d = st_current_segment->rate; + + // Check if the counters need to be reset for a new planner block + if (st.load_flag == LOAD_BLOCK) { + pl_current_block = plan_get_current_block(); // Should always be there. Stepper buffer handles this. + st_current_data = &segment_data[st_current_segment->st_data_index]; + + // Initialize direction bits for block + st.out_bits = pl_current_block->direction_bits ^ settings.invert_mask; + st.execute_step = true; // Set flag to set direction bits upon next ISR tick. + + // Initialize Bresenham line counters + st.counter_x = (pl_current_block->step_event_count >> 1); + st.counter_y = st.counter_x; + st.counter_z = st.counter_x; + + // Initialize inverse time and step rate counter data + st.counter_d = st_current_data->d_next; // d_next always greater than delta_d. + } + st.load_flag = LOAD_NOOP; // Motion loaded. Set no-operation flag until complete. + + } else { + // Can't discard planner block here if a feed hold stops in middle of block. + st_go_idle(); + bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end + return; // Nothing to do but exit. + } + + } + + // Iterate inverse time counter. Triggers each Bresenham step event. + st.counter_d -= st.delta_d; + + // Execute Bresenham step event, when it's time to do so. + if (st.counter_d < 0) { + st.counter_d += st_current_data->d_next; // Reload inverse time counter + + st.out_bits = pl_current_block->direction_bits; // Reset out_bits and reload direction bits + st.execute_step = true; + + // Execute step displacement profile by Bresenham line algorithm + st.counter_x -= pl_current_block->steps[X_AXIS]; + if (st.counter_x < 0) { + st.out_bits |= (1<step_event_count; + if (st.out_bits & (1<steps[Y_AXIS]; + if (st.counter_y < 0) { + st.out_bits |= (1<step_event_count; + if (st.out_bits & (1<steps[Z_AXIS]; + if (st.counter_z < 0) { + st.out_bits |= (1<step_event_count; + if (st.out_bits & (1<flag == ST_END_OF_BLOCK) { + plan_discard_current_block(); + st.load_flag = LOAD_BLOCK; + } else { + st.load_flag = LOAD_LINE; + } + + // Discard current block + if (segment_buffer_head != segment_buffer_tail) { + segment_buffer_tail = next_block_index( segment_buffer_tail ); + } + + // NOTE: sys.position updates could be done here. The bresenham counters can have + // their own fast 8-bit addition-only counters. Here we would check the direction and + // apply it to sys.position accordingly. However, this could take too much time. + + } + + st.out_bits ^= settings.invert_mask; // Apply step port invert mask + } + busy = false; +// SPINDLE_ENABLE_PORT ^= 1<step_events_remaining); + st.ramp_type = RAMP_ACCEL; + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; + st.delta_d = 0; + sys.state = STATE_QUEUED; + } else { + sys.state = STATE_IDLE; + } +} + + +/* Preps stepper buffer. Called from main program. + + NOTE: There doesn't seem to be a great way to figure out how many steps occur within + a set number of ISR ticks. Numerical round-off and CPU overhead always seems to be a + critical problem. So, either numerical round-off checks could be made to account for + them, while CPU overhead could be minimized in some way, or we can flip the algorithm + around to have the stepper algorithm track number of steps over an indeterminant amount + of time instead. + In other words, we use the planner velocity floating point data to get an estimate of + the number of steps we want to execute. We then back out the approximate velocity for + the planner to use, which should be much more robust to round-off error. The main problem + now is that we are loading the stepper algorithm to handle acceleration now, rather than + pre-calculating with the main program. This approach does make sense in the way that + planner velocities and stepper profiles can be traced more accurately. + Which is better? Very hard to tell. The time-based algorithm would be able to handle + Bresenham step adaptive-resolution much easier and cleaner. Whereas, the step-based would + require some additional math in the stepper algorithm to adjust on the fly, plus adaptation + would occur in a non-deterministic manner. + I suppose it wouldn't hurt to build both to see what's better. Just a lot more work. + + TODO: Need to describe the importance of continuations of step pulses between ramp states + and planner blocks. This has to do with Alden's problem with step "phase". The things I've + been doing here limit this phase issue by truncating some of the ramp timing for certain + events like deceleration initialization and end of block. +*/ +void st_prep_buffer() +{ + while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer. + + // Determine if we need to load a new planner block. + if (pl_prep_block == NULL) { + pl_prep_block = plan_get_block_by_index(pl_prep_index); + if (pl_prep_block == NULL) { return; } // No more planner blocks. Let stepper finish out. + + // Prepare commonly shared planner block data for the ensuing step buffer moves + st_data_prep_index = next_block_index(st_data_prep_index); + st_prep_data = &segment_data[st_data_prep_index]; + + // Initialize Bresenham variables + st_prep_data->step_events_remaining = pl_prep_block->step_event_count; + + // Convert new block to stepper variables. + // NOTE: This data can change mid-block from normal planner updates and feedrate overrides. Must + // be maintained as these execute. + // TODO: If the planner updates this block, particularly from a deceleration to an acceleration, + // we must reload the initial rate data, such that the velocity profile is re-constructed correctly. +// st_prep_data->initial_rate = ceil(sqrt(pl_prep_block->entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) +// st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + + // This data doesn't change. Could be performed in the planner, but fits nicely here. + // Although, acceleration can change for S-curves. So keep it here. +// st_prep_data->rate_delta = ceil(pl_prep_block->acceleration* +// ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) + // This definitely doesn't change, but could be precalculated in a way to help some of the + // math in this handler, i.e. millimeters per step event data. + st_prep_data->d_next = ceil((pl_prep_block->millimeters*INV_TIME_MULTIPLIER)/pl_prep_block->step_event_count); // (mult*mm/step) + + st_prep_data->mm_per_step = pl_prep_block->millimeters/pl_prep_block->step_event_count; + } + + + /* + // Check if planner has changed block exit parameters. If so, then we have to update the + // velocity profile for the remainder of this block. Otherwise, the block hasn't changed. + if (exit_speed_sqr != last_exit_speed_sqr) { + intersect_distance = 0.5*( millimeters + (entry_speed_sqr-exit_speed_sqr)/(2*acceleration) ); + if (intersect_distance <= 0) { // Deceleration only. + final_rate_sqr = initial_rate_sqr - 2*acceleration*segment_distance; + block->decelerate_after = 0; + } else { + decelerate_after = (block->nominal_speed_sqr - exit_speed_sqr)/(2*block->acceleration); + if (decelerate_after > intersect_distance) { decelerate_after = intersect_distance; } + if (decelerate_after > block->millimeters) { decelerate_after = block->millimeters; } + } + } + + + + + + n_step = 100; // Estimate distance we're going to travel in this segment + if (n_step > step_events_remaining) { n_step = step_events_remaining; }; + + segment_distance = n_step*mm_per_step; // True distance traveled + + // ISSUE: Either weighted average speed or truncated segments must be used. Otherwise + // accelerations, in particular high value, will not perform correctly. + if (distance_traveled < decelerate_after) { + if (segment_distance + distance_traveled > decelerate_after) { + n_step = ceil((decelerate_after-distance_traveled)/mm_per_step); + segment_distance = n_step*mm_per_step; + } + } + + + + + // based on time? + time = incremental time; + v_exit = v_entry + acceleration*time; + if (v_exit > v_nominal) { + time_accel = (v_nominal-v_entry)/acceleration; + distance = v_entry*time_accel + 0.5*acceleration*time_accel**2; + time_remaining = time - time_accel; + } + distance = v_entry*time + 0.5*acceleration*time*time; + + + t_accel = (v_nominal - v_entry) / acceleration; + t_decel = (v_exit - v_nominal) / -acceleration; + dt = (v_entry-v_exit)/acceleration; + if + + + + + // What's the speed of this segment? Computing per segment can get expensive, but this + // would allow S-curves to be installed pretty easily here. + if (initial_speed < nominal_speed) { + if (initial_distance < decelerate_after) { // Acceleration + exit_speed = sqrt(initial_speed*initial_speed + 2*acceleration*distance); + if (exit_speed > nominal_speed) { exit_speed = nominal_speed; } + } else { // Deceleration + exit_speed = sqrt(initial_speed*initial_speed - 2*acceleration*distance); + } + average_speed = 0.5*(initial_speed + exit_speed); + } else { // Cruise + average_speed = nominal_speed; + } + segment_rate = ceil(average_speed*(INV_TIME_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + if (segment_rate < MINIMUM_STEP_RATE) { segment_rate = MINIMUM_STEP_RATE; } + + + + + */ + + + + /* + TODO: Need to check for a planner flag to indicate a change to this planner block. + If so, need to check for a change in acceleration state, from deceleration to acceleration, + to reset the stepper ramp counters and the initial_rate data to trace the new + ac/de-celeration profile correctly. + No change conditions: + - From nominal speed to acceleration from feedrate override + - From nominal speed to new deceleration. + - From acceleration to new deceleration point later or cruising point. + - From acceleration to immediate deceleration? Can happen during feedrate override + and slowing down, but likely ok by enforcing the normal ramp counter protocol. + Change conditions: + - From deceleration to acceleration, i.e. common with jogging when new blocks are added. + */ + + st_segment_t *st_prep_block = &segment_buffer[segment_buffer_head]; + st_prep_block->st_data_index = st_data_prep_index; + + // TODO: How do you cheaply compute n_step without a sqrt()? Could be performed as 'bins'. + st_prep_block->n_step = 100; //floor( (exit_speed*approx_time)/mm_per_step ); +// st_segment->n_step = max(st_segment->n_step,MINIMUM_STEPS_PER_BLOCK); // Ensure it moves for very slow motions? +// st_segment->n_step = min(st_segment->n_step,MAXIMUM_STEPS_PER_BLOCK); // Prevent unsigned int8 overflow. + + // Check if n_step exceeds steps remaining in planner block. If so, truncate. + if (st_prep_block->n_step > st_prep_data->step_events_remaining) { + st_prep_block->n_step = st_prep_data->step_events_remaining; + } + + // Check if n_step exceeds decelerate point in block. Need to perform this so that the + // ramp counters are reset correctly in the stepper algorithm. Can be 1 step, but should + // be OK since it is likely moving at a fast rate already. + if (st_prep_block->n_step > pl_prep_block->decelerate_after) { + st_prep_block->n_step = pl_prep_block->decelerate_after; + } + + float distance, exit_speed_sqr; + distance = st_prep_block->n_step*st_prep_data->mm_per_step; // Always greater than zero + if (st_prep_data->step_events_remaining >= pl_prep_block->decelerate_after) { + exit_speed_sqr = pl_prep_block->entry_speed_sqr - 2*pl_prep_block->acceleration*distance; + // Set ISR tick reset flag for deceleration ramp. + } else { // Acceleration or cruising ramp + if (pl_prep_block->entry_speed_sqr < pl_prep_block->nominal_speed_sqr) { + exit_speed_sqr = pl_prep_block->entry_speed_sqr + 2*pl_prep_block->acceleration*distance; + if (exit_speed_sqr > pl_prep_block->nominal_speed_sqr) { exit_speed_sqr = pl_prep_block->nominal_speed_sqr; } + } else { + exit_speed_sqr = pl_prep_block->nominal_speed_sqr; + } + } + + + // Adjust inverse time counter for ac/de-celerations + if (st.ramp_type) { + st.ramp_count--; // Tick acceleration ramp counter + if (st.ramp_count == 0) { // Adjust step rate when its time + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter + if (st.ramp_type == RAMP_ACCEL) { // Adjust velocity for acceleration + st.delta_d += st_current_data->rate_delta; + if (st.delta_d >= st_current_data->nominal_rate) { // Reached nominal rate. + st.delta_d = st_current_data->nominal_rate; // Set cruising velocity + st.ramp_type = RAMP_NOOP_CRUISE; // Set ramp flag to ignore + } + } else { // Adjust velocity for deceleration + if (st.delta_d > st_current_data->rate_delta) { + st.delta_d -= st_current_data->rate_delta; + } else { + + // Moving near zero feed rate. Gracefully slow down. + st.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow. + + // Check for and handle feed hold exit? At this point, machine is stopped. + + } + } + // Finalize adjusted step rate. Ensure minimum. + if (st.delta_d < MINIMUM_STEP_RATE) { st.d_per_tick = MINIMUM_STEP_RATE; } + else { st.d_per_tick = st.delta_d; } + } + } + // During feed hold, do not update rate or ramp type. Keep decelerating. + if (sys.state == STATE_CYCLE) { + st.delta_d = st_current_data->initial_rate; + if (st.delta_d == st_current_data->nominal_rate) { + ramp_type = RAMP_NOOP_CRUISE; + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid + } + // Acceleration and cruise handled by ramping. Just check for deceleration. + if (st_current_segment->flag == ST_NOOP) { + if (st.ramp_type == RAMP_NOOP_CRUISE) { + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid + } else { + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; // Set ramp counter for triangle + } + st.ramp_type = RAMP_DECEL; + } + + + + + + // Update planner block variables. + pl_prep_block->entry_speed_sqr = max(0.0,exit_speed_sqr); +// pl_prep_block->max_entry_speed_sqr = exit_speed_sqr; // ??? Overwrites the corner speed. May need separate variable. + pl_prep_block->millimeters -= distance; // Potential round-off error near end of block. + pl_prep_block->millimeters = max(0.0,pl_prep_block->millimeters); // Shouldn't matter. + + // Update stepper block variables. + st_prep_data->step_events_remaining -= st_prep_block->n_step; + if ( st_prep_data->step_events_remaining == 0 ) { + // Move planner pointer to next block + st_prep_block->flag = ST_END_OF_BLOCK; + pl_prep_index = next_block_index(pl_prep_index); + pl_prep_block = NULL; + } else { + st_prep_block->flag = ST_NOOP; + } + + // New step block completed. Increment step buffer indices. + segment_buffer_head = segment_next_head; + segment_next_head = next_block_index(segment_buffer_head); + + } +} diff --git a/stepper_old.c b/stepper_old.c new file mode 100644 index 0000000..3191007 --- /dev/null +++ b/stepper_old.c @@ -0,0 +1,387 @@ +/* + stepper.c - stepper motor driver: executes motion plans using stepper motors + Part of Grbl + + Copyright (c) 2011-2013 Sungeun K. Jeon + Copyright (c) 2009-2011 Simen Svale Skogsrud + + 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 + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +/* The timer calculations of this module informed by the 'RepRap cartesian firmware' by Zack Smith + and Philipp Tiefenbacher. */ + +#include +#include "stepper.h" +#include "config.h" +#include "settings.h" +#include "planner.h" + +// Some useful constants +#define TICKS_PER_MICROSECOND (F_CPU/1000000) +#define CRUISE_RAMP 0 +#define ACCEL_RAMP 1 +#define DECEL_RAMP 2 + +// Stepper state variable. Contains running data and trapezoid variables. +typedef struct { + // Used by the bresenham line algorithm + int32_t counter_x, // Counter variables for the bresenham line tracer + counter_y, + counter_z; + int32_t event_count; // Total event count. Retained for feed holds. + int32_t step_events_remaining; // Steps remaining in motion + + // Used by Pramod Ranade inverse time algorithm + int32_t delta_d; // Ranade distance traveled per interrupt tick + int32_t d_counter; // Ranade distance traveled since last step event + uint8_t ramp_count; // Acceleration interrupt tick counter. + uint8_t ramp_type; // Ramp type variable. + uint8_t execute_step; // Flags step execution for each interrupt. + +} stepper_t; +static stepper_t st; +static block_t *current_block; // A pointer to the block currently being traced + +// Used by the stepper driver interrupt +static uint8_t step_pulse_time; // Step pulse reset time after step rise +static uint8_t out_bits; // The next stepping-bits to be output + +// NOTE: If the main interrupt is guaranteed to be complete before the next interrupt, then +// this blocking variable is no longer needed. Only here for safety reasons. +static volatile uint8_t busy; // True when "Stepper Driver Interrupt" is being serviced. Used to avoid retriggering that handler. + +// __________________________ +// /| |\ _________________ ^ +// / | | \ /| |\ | +// / | | \ / | | \ s +// / | | | | | \ p +// / | | | | | \ e +// +-----+------------------------+---+--+---------------+----+ e +// | BLOCK 1 | BLOCK 2 | d +// +// time -----> +// +// The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta +// until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after +// after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as +// +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. + + +// Stepper state initialization. Cycle should only start if the st.cycle_start flag is +// enabled. Startup init and limits call this function but shouldn't start the cycle. +void st_wake_up() +{ + // Enable steppers by resetting the stepper disable port + if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { + STEPPERS_DISABLE_PORT |= (1<> 3); + // Enable stepper driver interrupt + st.execute_step = false; + TCNT2 = 0; // Clear Timer2 + TIMSK2 |= (1<direction_bits ^ settings.invert_mask; + st.execute_step = true; // Set flag to set direction bits. + + // Initialize Bresenham variables + st.counter_x = (current_block->step_event_count >> 1); + st.counter_y = st.counter_x; + st.counter_z = st.counter_x; + st.event_count = current_block->step_event_count; + st.step_events_remaining = st.event_count; + + // During feed hold, do not update Ranade counter, rate, or ramp type. Keep decelerating. + if (sys.state == STATE_CYCLE) { + // Initialize Ranade variables + st.d_counter = current_block->d_next; + st.delta_d = current_block->initial_rate; + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; + + // Initialize ramp type. + if (st.step_events_remaining == current_block->decelerate_after) { st.ramp_type = DECEL_RAMP; } + else if (st.delta_d == current_block->nominal_rate) { st.ramp_type = CRUISE_RAMP; } + else { st.ramp_type = ACCEL_RAMP; } + } + + } else { + st_go_idle(); + bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end + return; // Nothing to do but exit. + } + } + + // Adjust inverse time counter for ac/de-celerations + if (st.ramp_type) { + // Tick acceleration ramp counter + st.ramp_count--; + if (st.ramp_count == 0) { + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter + if (st.ramp_type == ACCEL_RAMP) { // Adjust velocity for acceleration + st.delta_d += current_block->rate_delta; + if (st.delta_d >= current_block->nominal_rate) { // Reached cruise state. + st.ramp_type = CRUISE_RAMP; + st.delta_d = current_block->nominal_rate; // Set cruise velocity + } + } else if (st.ramp_type == DECEL_RAMP) { // Adjust velocity for deceleration + if (st.delta_d > current_block->rate_delta) { + st.delta_d -= current_block->rate_delta; + } else { + st.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow. + } + } + } + } + + // Iterate Pramod Ranade inverse time counter. Triggers each Bresenham step event. + if (st.delta_d < MINIMUM_STEP_RATE) { st.d_counter -= MINIMUM_STEP_RATE; } + else { st.d_counter -= st.delta_d; } + + // Execute Bresenham step event, when it's time to do so. + if (st.d_counter < 0) { + st.d_counter += current_block->d_next; + + // Check for feed hold state and execute accordingly. + if (sys.state == STATE_HOLD) { + if (st.ramp_type != DECEL_RAMP) { + st.ramp_type = DECEL_RAMP; + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; + } + if (st.delta_d <= current_block->rate_delta) { + st_go_idle(); + bit_true(sys.execute,EXEC_CYCLE_STOP); + return; + } + } + + // TODO: Vary Bresenham resolution for smoother motions or enable faster step rates (>20kHz). + + out_bits = current_block->direction_bits; // Reset out_bits and reload direction bits + st.execute_step = true; + + // Execute step displacement profile by Bresenham line algorithm + st.counter_x -= current_block->steps[X_AXIS]; + if (st.counter_x < 0) { + out_bits |= (1<steps[Y_AXIS]; + if (st.counter_y < 0) { + out_bits |= (1<steps[Z_AXIS]; + if (st.counter_z < 0) { + out_bits |= (1<decelerate_after) { + st.ramp_type = DECEL_RAMP; + if (st.step_events_remaining == current_block->decelerate_after) { + if (st.delta_d == current_block->nominal_rate) { + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid + } else { + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; // Set ramp counter for triangle + } + } + } + } + } else { + // If current block is finished, reset pointer + current_block = NULL; + plan_discard_current_block(); + } + + out_bits ^= settings.invert_mask; // Apply step port invert mask + } + busy = false; +// SPINDLE_ENABLE_PORT ^= 1< Date: Wed, 9 Oct 2013 09:33:22 -0600 Subject: [PATCH 13/73] Protected buffer works! Vast improvements to planner efficiency. Many things still broken with overhaul. Development push. Lots still broken. - Protected planner concept works! This is a critical precursor to enabling feedrate overrides in allowing the planner buffer and the stepper execution operate atomically. This is done through a intermediary segment buffer. - Still lots of work to be done, as this was a complete overhaul of the planner and stepper subsystems. The code can be cleaned up quite a bit, re-enabling some of the broken features like feed holds, and finishing up some of the concepts - Pushed some of the fixes from the master and edge branch to here, as this will likely replace the edge branch when done. --- Makefile | 2 +- config.h | 93 +++------- doc/pinmapping.txt | 97 +++++++++++ nuts_bolts.h | 1 + pin_map.h | 181 ++++++++++++++++++++ planner.c | 417 ++++++++++++++++++++++++++++++++------------- planner.h | 20 ++- serial.c | 12 +- stepper.c | 267 +++++++++++++++++++---------- stepper.h | 4 + 10 files changed, 793 insertions(+), 301 deletions(-) create mode 100644 doc/pinmapping.txt create mode 100644 pin_map.h diff --git a/Makefile b/Makefile index f0e7f41..78c1032 100644 --- a/Makefile +++ b/Makefile @@ -84,7 +84,7 @@ main.elf: $(OBJECTS) grbl.hex: main.elf rm -f grbl.hex avr-objcopy -j .text -j .data -O ihex main.elf grbl.hex - avr-size -C --mcu=$(DEVICE) main.elf + avr-size --format=berkeley main.elf # If you have an EEPROM section, you must also create a hex file for the # EEPROM and add it to the "flash" target. diff --git a/config.h b/config.h index c1c450e..41d6a38 100644 --- a/config.h +++ b/config.h @@ -2,8 +2,8 @@ config.h - compile time configuration Part of Grbl - Copyright (c) 2009-2011 Simen Svale Skogsrud Copyright (c) 2011-2013 Sungeun K. Jeon + Copyright (c) 2009-2011 Simen Svale Skogsrud Grbl is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -19,80 +19,24 @@ along with Grbl. If not, see . */ -#ifndef config_h -#define config_h +// This file contains compile-time configurations for Grbl's internal system. For the most part, +// users will not need to directly modify these, but they are here for specific needs, i.e. +// performance tuning or adjusting to non-typical machines. // IMPORTANT: Any changes here requires a full re-compiling of the source code to propagate them. +#ifndef config_h +#define config_h + // Default settings. Used when resetting EEPROM. Change to desired name in defaults.h #define DEFAULTS_ZEN_TOOLWORKS_7x7 // Serial baud rate -#define BAUD_RATE 9600 +#define BAUD_RATE 115200 -// Define pin-assignments -// NOTE: All step bit and direction pins must be on the same port. -#define STEPPING_DDR DDRD -#define STEPPING_PORT PORTD -#define X_STEP_BIT 2 // Uno Digital Pin 2 -#define Y_STEP_BIT 3 // Uno Digital Pin 3 -#define Z_STEP_BIT 4 // Uno Digital Pin 4 -#define X_DIRECTION_BIT 5 // Uno Digital Pin 5 -#define Y_DIRECTION_BIT 6 // Uno Digital Pin 6 -#define Z_DIRECTION_BIT 7 // Uno Digital Pin 7 -#define STEP_MASK ((1< #include "config.h" #include "defaults.h" +#include "pin_map.h" #define false 0 #define true 1 diff --git a/pin_map.h b/pin_map.h new file mode 100644 index 0000000..0167cc9 --- /dev/null +++ b/pin_map.h @@ -0,0 +1,181 @@ +/* + pin_map.h - Pin mapping configuration file + Part of Grbl + + Copyright (c) 2013 Sungeun K. 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 + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +/* The pin_map.h file serves as a central pin mapping settings file for different processor + types, i.e. AVR 328p or AVR Mega 2560. Grbl officially supports the Arduino Uno, but the + other supplied pin mappings are supplied by users, so your results may vary. */ + +#ifndef pin_map_h +#define pin_map_h + +#ifdef PIN_MAP_ARDUINO_UNO // AVR 328p, Officially supported by Grbl. + + // Serial port pins + #define SERIAL_RX USART_RX_vect + #define SERIAL_UDRE USART_UDRE_vect + + // NOTE: All step bit and direction pins must be on the same port. + #define STEPPING_DDR DDRD + #define STEPPING_PORT PORTD + #define X_STEP_BIT 2 // Uno Digital Pin 2 + #define Y_STEP_BIT 3 // Uno Digital Pin 3 + #define Z_STEP_BIT 4 // Uno Digital Pin 4 + #define X_DIRECTION_BIT 5 // Uno Digital Pin 5 + #define Y_DIRECTION_BIT 6 // Uno Digital Pin 6 + #define Z_DIRECTION_BIT 7 // Uno Digital Pin 7 + #define STEP_MASK ((1<entry_speed_sqr = exit_speed_sqr - 2*partial_block->acceleration*millimeters_remaining; + } else { // Block is accelerating or cruising + partial_block->entry_speed_sqr += 2*partial_block->acceleration*(partial_block->millimeters-millimeters_remaining); + partial_block->entry_speed_sqr = min(partial_block->entry_speed_sqr, partial_block->nominal_speed_sqr); + } + + // Update only the relevant planner block information so the planner can plan correctly. + partial_block->millimeters = millimeters_remaining; + partial_block->max_entry_speed_sqr = partial_block->entry_speed_sqr; // Not sure if this needs to be updated. + } +} + + + + /* PLANNER SPEED DEFINITION +--------+ <- current->nominal_speed / \ current->entry_speed -> + \ - | + <- next->entry_speed + | + <- next->entry_speed (aka exit speed) +-------------+ time --> @@ -112,7 +153,7 @@ static uint8_t prev_block_index(uint8_t block_index) in the entire buffer to accelerate up to the nominal speed and then decelerate to a stop at the end of the buffer. There are a few simple solutions to this: (1) Maximize the machine acceleration. The planner will be able to compute higher speed profiles within the same combined distance. (2) Increase line segment(s) distance. - The more combined distance the planner has to use, the faster it can go. (3) Increase the MINIMUM_PLANNER_SPEED. + The more combined distance the planner has to use, the faster it can go. (3) Increase the MINIMUM_JUNCTION_SPEED. Not recommended. This will change what speed the planner plans to at the end of the buffer. Can lead to lost steps when coming to a stop. (4) [BEST] Increase the planner buffer size. The more combined distance, the bigger the balloon, or faster it can go. But this is not possible for 328p Arduinos because its limited memory @@ -123,69 +164,178 @@ static uint8_t prev_block_index(uint8_t block_index) as possible. For example, in situations like arc generation or complex curves, the short, rapid line segments can execute faster than new blocks can be added, and the planner buffer will then starve and empty, leading to weird hiccup-like jerky motions. + + Index mapping: + - block_buffer_head: Points to the newest incoming buffer block just added by plan_buffer_line(). The planner + never touches the exit speed of this block, which always defaults to MINIMUM_JUNCTION_SPEED. + - block_buffer_tail: Points to the beginning of the planner buffer. First to be executed or being executed. + Can dynamically change with the old stepper algorithm, but with the new algorithm, this should be impossible + as long as the segment buffer is not empty. + - next_buffer_head: Points to next planner buffer block after the last block. Should always be empty. + - block_buffer_safe: Points to the first planner block in the buffer for which it is safe to change. Since + the stepper can be executing the first block and if the planner changes its conditions, this will cause + a discontinuity and error in the stepper profile with lost steps likely. With the new stepper algorithm, + the block_buffer_safe is always where the stepper segment buffer ends and can never be overwritten, but + this can change the state of the block profile from a pure trapezoid assumption. Meaning, if that block + is decelerating, the planner conditions can change such that the block can new accelerate mid-block. + + !!! I need to make sure that the stepper algorithm can modify the acceleration mid-block. Needed for feedrate overrides too. + + !!! planner_recalculate() may not work correctly with re-planning.... may need to artificially set both the + block_buffer_head and next_buffer_head back one index so that this works correctly, or allow the operation + of this function to accept two different conditions to operate on. + + - block_buffer_planned: Points to the first buffer block after the last optimally fixed block, which can no longer be + improved. This block and the trailing buffer blocks that can still be altered when new blocks are added. This planned + block points to the transition point between the fixed and non-fixed states and is handled slightly different. The entry + speed is fixed, indicating the reverse pass cannot maximize the speed further, but the velocity profile within it + can still be changed, meaning the forward pass calculations must start from here and influence the following block + entry speed. + + !!! Need to check if this is the start of the non-optimal or the end of the optimal block. */ static void planner_recalculate() -{ - // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated. - uint8_t block_index = block_buffer_head; - plan_block_t *current = &block_buffer[block_index]; // Set as last/newest block in buffer +{ + // Query stepper module for safe planner block index to recalculate to, which corresponds to the end + // of the step segment buffer. + uint8_t block_buffer_safe = st_get_prep_block_index(); + // TODO: Make sure that we don't have to check for the block_buffer_tail condition, if the stepper module + // returns a NULL pointer or something. This could happen when the segment buffer is empty. Although, + // this call won't return a NULL, only an index.. I have to make sure that this index is synced with the + // planner at all times. - // Ping the stepper algorithm to check if we can alter the parameters of the currently executing - // block. If not, skip it and work on the next block. - // TODO: Need to look into if there are conditions where this fails. - uint8_t block_buffer_safe = next_block_index( block_buffer_tail ); - - // TODO: Need to recompute buffer tail millimeters based on how much is completed. - - if (block_buffer_safe == next_buffer_head) { // Only one safe block in buffer to operate on + /* - In theory, the state of the segment buffer can exist anywhere within the planner buffer tail and head-1 + or is empty, when there is nothing in the segment queue. The safe pointer can be the buffer head only + when the planner queue has been entirely queued into the segment buffer and there are no more blocks + in the planner buffer. The segment buffer will to continue to execute the remainder of it, but the + planner should be able to treat a newly added block during this time as an empty planner buffer since + we can't touch the segment buffer. + + - The segment buffer is atomic to the planner buffer, because the main program computes these seperately. + Even if we move the planner head pointer early at the end of plan_buffer_line(), this shouldn't + effect the safe pointer. - block_buffer_planned = block_buffer_safe; -// calculate_trapezoid_for_block(current, 0.0, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED); + - If the safe pointer is at head-1, this means that the stepper algorithm has segments queued and may + be executing. This is the last block in the planner queue, so it has been planned to decelerate to + zero at its end. When adding a new block, there will be at least two blocks to work with. When resuming, + from a feed hold, we only have this block and will be computing nothing. The planner doesn't have to + do anything, since the trapezoid calculations called by the stepper module should complete the block plan. + + - In most cases, the safe pointer is at the plan tail or the block after, and rarely on the block two + beyond the tail. Since the safe pointer points to the block used at the end of the segment buffer, it + can be in any one of these states. As the stepper module executes the planner block, the buffer tail, + and hence the safe pointer, can push forward through the planner blocks and overcome the planned + pointer at any time. + - Does the reverse pass not touch either the safe or the plan pointer blocks? The plan pointer only + allows the velocity profile within it to be altered, but not the entry speed, so the reverse pass + ignores this block. The safe pointer is the same way, where the entry speed does not change, but + the velocity profile within it does. + + - The planned pointer can exist anywhere in a given plan, except for the planner buffer head, if everything + operates as anticipated. Since the planner buffer can be executed by the stepper algorithm as any + rate and could empty the planner buffer quickly, the planner tail can overtake the planned pointer + at any time, but will never go around the ring buffer and re-encounter itself, the plan itself is not + changed by adding a new block or something else. + + - The planner recalculate function should always reset the planned pointer at the proper break points + or when it encounters the safe block pointer, but will only do so when there are more than one block + in the buffer. In the case of single blocks, the planned pointer should always be set to the first + write-able block in the buffer, aka safe block. + + - When does this not work? There might be an issue when the planned pointer moves from the tail to the + next head as a new block is being added and planned. Otherwise, the planned pointer should remain + static within the ring buffer no matter what the buffer is doing: being executed, adding new blocks, + or both simultaneously. Need to make sure that this case is covered. + */ + + + // Recompute plan only when there is more than one planner block in the buffer. Can't do anything with one. + // NOTE: block_buffer_safe can be equal to block_buffer_head if the segment buffer has completely queued up + // the remainder of the planner buffer. In this case, a new planner block will be treated as a single block. + if (block_buffer_head == block_buffer_safe) { // Also catches head = tail + + // Just set block_buffer_planned pointer. + block_buffer_planned = block_buffer_head; + printString("z"); + + // TODO: Feedrate override of one block needs to update the partial block with an exit speed of zero. For + // a single added block and recalculate after a feed hold, we don't need to compute this, since we already + // know that the velocity starts and ends at zero. With an override, we can be traveling at some midblock + // rate, and we have to calculate the new velocity profile from it. + // plan_update_partial_block(block_index,0.0); + } else { - - // TODO: need to account for the two block condition better. If the currently executing block - // is not safe, do we wait until its done? Can we treat the buffer head differently? - - // Calculate trapezoid for the last/newest block. - current->entry_speed_sqr = min( current->max_entry_speed_sqr, - MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED + 2*current->acceleration*current->millimeters); -// calculate_trapezoid_for_block(current, current->entry_speed_sqr, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED); + + // TODO: If the nominal speeds change during a feedrate override, we need to recompute the max entry speeds for + // all junctions before proceeding. + // Initialize planner buffer pointers and indexing. + uint8_t block_index = block_buffer_head; + plan_block_t *current = &block_buffer[block_index]; + + // Calculate maximum entry speed for last block in buffer, where the exit speed is always zero. + current->entry_speed_sqr = min( current->max_entry_speed_sqr, 2*current->acceleration*current->millimeters); - // Reverse Pass: Back plan the deceleration curve from the last block in buffer. Cease - // planning when: (1) the last optimal planned pointer is reached. (2) the safe block - // pointer is reached, whereby the planned pointer is updated. + // Reverse Pass: Coarsely maximize all possible deceleration curves back-planning from the last + // block in buffer. Cease planning when: (1) the last optimal planned pointer is reached. + // (2) the safe block pointer is reached, whereby the planned pointer is updated. + // NOTE: Forward pass will later refine and correct the reverse pass to create an optimal plan. + // NOTE: If the safe block is encountered before the planned block pointer, we know the safe block + // will be recomputed within the plan. So, we need to update it if it is partially completed. float entry_speed_sqr; plan_block_t *next; block_index = prev_block_index(block_index); - while (block_index != block_buffer_planned) { - next = current; - current = &block_buffer[block_index]; + + if (block_index == block_buffer_safe) { // !! OR plan pointer? Yes I think so. - // Exit loop and update planned pointer when the tail/safe block is reached. - if (block_index == block_buffer_safe) { - block_buffer_planned = block_buffer_safe; - break; - } + // Only two plannable blocks in buffer. Compute previous block based on + // !!! May only work if a new block is being added. Not for an override. The exit speed isn't zero. + // !!! Need to make the current entry speed calculation after this. + plan_update_partial_block(block_index, 0.0); + block_buffer_planned = block_index; +printString("y"); + + } else { - // Crudely maximize deceleration curve from the end of the non-optimally planned buffer to - // the optimal plan pointer. Forward pass will adjust and finish optimizing the plan. - if (current->entry_speed_sqr != current->max_entry_speed_sqr) { - entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters; - if (entry_speed_sqr < current->max_entry_speed_sqr) { - current->entry_speed_sqr = entry_speed_sqr; - } else { - current->entry_speed_sqr = current->max_entry_speed_sqr; + // Three or more plan-able + while (block_index != block_buffer_planned) { + + next = current; + current = &block_buffer[block_index]; + + // Increment block index early to check if the safe block is before the current block. If encountered, + // this is an exit condition as we can't go further than this block in the reverse pass. + block_index = prev_block_index(block_index); + if (block_index == block_buffer_safe) { + // Check if the safe block is partially completed. If so, update it before its exit speed + // (=current->entry speed) is over-written. + // TODO: The update breaks with feedrate overrides, because the replanning process no longer has + // the previous nominal speed to update this block with. There will need to be something along the + // lines of a nominal speed change check and send the correct value to this function. + plan_update_partial_block(block_index,current->entry_speed_sqr); +printString("x"); + // Set planned pointer at safe block and for loop exit after following computation is done. + block_buffer_planned = block_index; + } + + // Compute maximum entry speed decelerating over the current block from its exit speed. + if (current->entry_speed_sqr != current->max_entry_speed_sqr) { + entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters; + if (entry_speed_sqr < current->max_entry_speed_sqr) { + current->entry_speed_sqr = entry_speed_sqr; + } else { + current->entry_speed_sqr = current->max_entry_speed_sqr; + } } } - block_index = prev_block_index(block_index); - } + + } // Forward Pass: Forward plan the acceleration curve from the planned pointer onward. // Also scans for optimal plan breakpoints and appropriately updates the planned pointer. - block_index = block_buffer_planned; // Begin at buffer planned pointer - next = &block_buffer[prev_block_index(block_buffer_planned)]; // Set up for while loop + next = &block_buffer[block_buffer_planned]; // Begin at buffer planned pointer + block_index = next_block_index(block_buffer_planned); while (block_index != next_buffer_head) { current = next; next = &block_buffer[block_index]; @@ -194,22 +344,22 @@ static void planner_recalculate() // pointer forward, since everything before this is all optimal. In other words, nothing // can improve the plan from the buffer tail to the planned pointer by logic. if (current->entry_speed_sqr < next->entry_speed_sqr) { - block_buffer_planned = block_index; entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters; + // If true, current block is full-acceleration and we can move the planned pointer forward. if (entry_speed_sqr < next->entry_speed_sqr) { - next->entry_speed_sqr = entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass set this. + next->entry_speed_sqr = entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass sets this. + block_buffer_planned = block_index; // Set optimal plan pointer. } } // Any block set at its maximum entry speed also creates an optimal plan up to this - // point in the buffer. The optimally planned pointer is updated. + // point in the buffer. When the plan is bracketed by either the beginning of the + // buffer and a maximum entry speed or two maximum entry speeds, every block in between + // cannot logically be further improved. Hence, we don't have to recompute them anymore. if (next->entry_speed_sqr == next->max_entry_speed_sqr) { - block_buffer_planned = block_index; + block_buffer_planned = block_index; // Set optimal plan pointer } - // Automatically recalculate trapezoid for all buffer blocks from last plan's optimal planned - // pointer to the end of the buffer, except the last block. -// calculate_trapezoid_for_block(current, current->entry_speed_sqr, next->entry_speed_sqr); block_index = next_block_index( block_index ); } @@ -218,19 +368,24 @@ static void planner_recalculate() } +void plan_reset_buffer() +{ + block_buffer_planned = block_buffer_tail; +} + void plan_init() { - block_buffer_head = 0; - block_buffer_tail = block_buffer_head; - next_buffer_head = next_block_index(block_buffer_head); - block_buffer_planned = block_buffer_head; + block_buffer_tail = 0; + block_buffer_head = 0; // Empty = tail + next_buffer_head = 1; // next_block_index(block_buffer_head) + plan_reset_buffer(); memset(&pl, 0, sizeof(pl)); // Clear planner struct } void plan_discard_current_block() { - if (block_buffer_head != block_buffer_tail) { + if (block_buffer_head != block_buffer_tail) { // Discard non-empty buffer. block_buffer_tail = next_block_index( block_buffer_tail ); } } @@ -238,7 +393,10 @@ void plan_discard_current_block() plan_block_t *plan_get_current_block() { - if (block_buffer_head == block_buffer_tail) { return(NULL); } + if (block_buffer_head == block_buffer_tail) { // Buffer empty + plan_reset_buffer(); + return(NULL); + } return(&block_buffer[block_buffer_tail]); } @@ -289,6 +447,8 @@ void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) block->acceleration = SOME_LARGE_VALUE; // Scaled down to maximum acceleration later // Compute and store initial move distance data. + // TODO: After this for-loop, we don't touch the stepper algorithm data. Might be a good idea + // to try to keep these types of things completely separate from the planner for portability. int32_t target_steps[N_AXIS]; float unit_vec[N_AXIS], delta_mm; uint8_t idx; @@ -313,7 +473,7 @@ void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) } block->millimeters = sqrt(block->millimeters); // Complete millimeters calculation with sqrt() - // Bail if this is a zero-length block + // Bail if this is a zero-length block. Highly unlikely to occur. if (block->step_event_count == 0) { return; } // Adjust feed_rate value to mm/min depending on type of rate input (normal, inverse time, or rapids) @@ -346,41 +506,59 @@ void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) } } - /* Compute maximum allowable entry speed at junction by centripetal acceleration approximation. - 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 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. This approach does not actually deviate - from path, but used as a robust way to compute cornering speeds, as it takes into account the - nonlinearities of both the junction angle and junction velocity. - NOTE: If the junction deviation value is finite, Grbl executes the motions in an exact path - mode (G61). If the junction deviation value is zero, Grbl will execute the motion in an exact - stop mode (G61.1) manner. In the future, if continuous mode (G64) is desired, the math here - is exactly the same. Instead of motioning all the way to junction point, the machine will - just follow the arc circle defined here. The Arduino doesn't have the CPU cycles to perform - a continuous mode path, but ARM-based microcontrollers most certainly do. - */ - // TODO: Acceleration need to be limited by the minimum of the two junctions. - // TODO: Need to setup a method to handle zero junction speeds when starting from rest. + + // TODO: Need to check this method handling zero junction speeds when starting from rest. if (block_buffer_head == block_buffer_tail) { - block->max_entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED; + + // Initialize block entry speed as zero. Assume it will be starting from rest. Planner will correct this later. + // !!! Ensures when the first block starts from zero speed. If we do this in the planner, this will break + // feedrate overrides later, as you can override this single block and it maybe moving already at a given rate. + // Better to do it here and make it clean. + // !!! Shouldn't need this for anything other than a single block. + block->entry_speed_sqr = 0.0; + block->max_junction_speed_sqr = 0.0; // Starting from rest. Enforce start from zero velocity. + } else { + /* + Compute maximum allowable entry speed at junction by centripetal acceleration approximation. + 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 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. This approach does not actually deviate + from path, but used as a robust way to compute cornering speeds, as it takes into account the + nonlinearities of both the junction angle and junction velocity. + + NOTE: If the junction deviation value is finite, Grbl executes the motions in an exact path + mode (G61). If the junction deviation value is zero, Grbl will execute the motion in an exact + stop mode (G61.1) manner. In the future, if continuous mode (G64) is desired, the math here + is exactly the same. Instead of motioning all the way to junction point, the machine will + just follow the arc circle defined here. The Arduino doesn't have the CPU cycles to perform + a continuous mode path, but ARM-based microcontrollers most certainly do. + + NOTE: The max junction speed is a fixed value, since machine acceleration limits cannot be + changed dynamically during operation nor can the line segment geometry. This must be kept in + memory in the event of a feedrate override changing the nominal speeds of blocks, which can + change the overall maximum entry speed conditions of all blocks. + + */ // NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta). float sin_theta_d2 = sqrt(0.5*(1.0-junction_cos_theta)); // Trig half angle identity. Always positive. - block->max_entry_speed_sqr = (block->acceleration * settings.junction_deviation * sin_theta_d2)/(1.0-sin_theta_d2); + + // TODO: Acceleration used in calculation needs to be limited by the minimum of the two junctions. + block->max_junction_speed_sqr = max( MINIMUM_JUNCTION_SPEED*MINIMUM_JUNCTION_SPEED, + (block->acceleration * settings.junction_deviation * sin_theta_d2)/(1.0-sin_theta_d2) ); } - // Store block nominal speed and rate + // Store block nominal speed block->nominal_speed_sqr = feed_rate*feed_rate; // (mm/min). Always > 0 -// block->nominal_rate = ceil(feed_rate*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) -// -// // Compute and store acceleration and distance traveled per step event. -// block->rate_delta = ceil(block->acceleration* -// ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) -// block->d_next = ceil((block->millimeters*INV_TIME_MULTIPLIER)/block->step_event_count); // (mult*mm/step) - + + // Compute the junction maximum entry based on the minimum of the junction speed and neighboring nominal speeds. + // TODO: Should call a function to determine this. The function can be used elsewhere for feedrate overrides later. + block->max_entry_speed_sqr = min(block->max_junction_speed_sqr, + min(block->nominal_speed_sqr,pl.previous_nominal_speed_sqr)); + // Update previous path unit_vector and nominal speed (squared) memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // pl.previous_unit_vec[] = unit_vec[] pl.previous_nominal_speed_sqr = block->nominal_speed_sqr; @@ -390,11 +568,17 @@ void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) planner_recalculate(); - // Update buffer head and next buffer head indices. - // NOTE: The buffer head update is atomic since it's one byte. Performed after the new plan - // calculations to help prevent overwriting scenarios with adding a new block to a low buffer. + // Update buffer head and next buffer head indices. Advance only after new plan has been computed. block_buffer_head = next_buffer_head; next_buffer_head = next_block_index(block_buffer_head); + + + +int32_t blength = block_buffer_head - block_buffer_tail; +if (blength < 0) { blength += BLOCK_BUFFER_SIZE; } +printInteger(blength); + + } @@ -408,6 +592,8 @@ void plan_sync_position() } + + /* STEPPER VELOCITY PROFILE DEFINITION less than nominal rate-> + +--------+ <- nominal_rate /|\ @@ -419,31 +605,35 @@ void plan_sync_position() | | | | decelerate distance decelerate distance - Calculates trapezoid parameters for stepper algorithm. Each block velocity profiles can be - described as either a trapezoidal or a triangular shape. The trapezoid occurs when the block - reaches the nominal speed of the block and cruises for a period of time. A triangle occurs - when the nominal speed is not reached within the block. Some other special cases exist, - such as pure ac/de-celeration velocity profiles from beginning to end or a trapezoid that - has no deceleration period when the next block resumes acceleration. + Calculates the "trapezoid" velocity profile parameters of a planner block for the stepper + algorithm. The planner computes the entry and exit speeds of each block, but does not bother to + determine the details of the velocity profiles within them, as they aren't needed for computing + an optimal plan. When the stepper algorithm begins to execute a block, the block velocity profiles + are computed ad hoc. + + Each block velocity profiles can be described as either a trapezoidal or a triangular shape. The + trapezoid occurs when the block reaches the nominal speed of the block and cruises for a period of + time. A triangle occurs when the nominal speed is not reached within the block. Both of these + velocity profiles may also be truncated on either end with no acceleration or deceleration ramps, + as they can be influenced by the conditions of neighboring blocks. The following function determines the type of velocity profile and stores the minimum required - information for the stepper algorithm to execute the calculated profiles. In this case, only - the new initial rate and n_steps until deceleration are computed, since the stepper algorithm - already handles acceleration and cruising and just needs to know when to start decelerating. + information for the stepper algorithm to execute the calculated profiles. Since the stepper + algorithm always assumes to begin accelerating from the initial_rate and cruise if the nominal_rate + is reached, we only need to know when to begin deceleration to the end of the block. Hence, only + the distance from the end of the block to begin a deceleration ramp are computed. */ -int32_t calculate_trapezoid_for_block(uint8_t block_index) +float plan_calculate_velocity_profile(uint8_t block_index) { plan_block_t *current_block = &block_buffer[block_index]; // Determine current block exit speed - float exit_speed_sqr; - uint8_t next_index = next_block_index(block_index); - plan_block_t *next_block = plan_get_block_by_index(next_index); - if (next_block == NULL) { exit_speed_sqr = 0; } // End of planner buffer. Zero speed. - else { exit_speed_sqr = next_block->entry_speed_sqr; } // Entry speed of next block + float exit_speed_sqr = 0.0; // Initialize for end of planner buffer. Zero speed. + plan_block_t *next_block = plan_get_block_by_index(next_block_index(block_index)); + if (next_block != NULL) { exit_speed_sqr = next_block->entry_speed_sqr; } // Exit speed is the entry speed of next buffer block // First determine intersection distance (in steps) from the exit point for a triangular profile. - // Computes: steps_intersect = steps/mm * ( distance/2 + (v_entry^2-v_exit^2)/(4*acceleration) ) + // Computes: d_intersect = distance/2 + (v_entry^2-v_exit^2)/(4*acceleration) float intersect_distance = 0.5*( current_block->millimeters + (current_block->entry_speed_sqr-exit_speed_sqr)/(2*current_block->acceleration) ); // Check if this is a pure acceleration block by a intersection distance less than zero. Also @@ -452,18 +642,17 @@ int32_t calculate_trapezoid_for_block(uint8_t block_index) float decelerate_distance; // Determine deceleration distance (in steps) from nominal speed to exit speed for a trapezoidal profile. // Value is never negative. Nominal speed is always greater than or equal to the exit speed. - // Computes: steps_decelerate = steps/mm * ( (v_nominal^2 - v_exit^2)/(2*acceleration) ) + // Computes: d_decelerate = (v_nominal^2 - v_exit^2)/(2*acceleration) decelerate_distance = (current_block->nominal_speed_sqr - exit_speed_sqr)/(2*current_block->acceleration); // The lesser of the two triangle and trapezoid distances always defines the velocity profile. if (decelerate_distance > intersect_distance) { decelerate_distance = intersect_distance; } // Finally, check if this is a pure deceleration block. - if (decelerate_distance > current_block->millimeters) { decelerate_distance = current_block->millimeters; } - - return(ceil(((current_block->millimeters-decelerate_distance)*current_block->step_event_count)/ current_block->millimeters)); + if (decelerate_distance > current_block->millimeters) { return(0.0); } + else { return( (current_block->millimeters-decelerate_distance) ); } } - return(0); + return( current_block->millimeters ); // No deceleration in velocity profile. } @@ -481,7 +670,7 @@ void plan_cycle_reinitialize(int32_t step_events_remaining) // Re-plan from a complete stop. Reset planner entry speeds and buffer planned pointer. block->entry_speed_sqr = 0.0; - block->max_entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED; + block->max_entry_speed_sqr = 0.0; block_buffer_planned = block_buffer_tail; planner_recalculate(); } diff --git a/planner.h b/planner.h index e371bb0..083d182 100644 --- a/planner.h +++ b/planner.h @@ -33,20 +33,20 @@ typedef struct { // Fields used by the bresenham algorithm for tracing the line + // NOTE: Do not change any of these values once set. The stepper algorithm uses them to execute the block correctly. uint8_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) int32_t steps[N_AXIS]; // Step count along each axis - int32_t step_event_count; // The number of step events required to complete this block + int32_t step_event_count; // The maximum step axis count and number of steps required to complete this block. // Fields used by the motion planner to manage acceleration + float entry_speed_sqr; // The current planned entry speed at block junction in (mm/min)^2 + float max_entry_speed_sqr; // Maximum allowable entry speed based on the minimum of junction limit and + // neighboring nominal speeds with overrides in (mm/min)^2 + float max_junction_speed_sqr; // Junction entry speed limit based on direction vectors in (mm/min)^2 float nominal_speed_sqr; // Axis-limit adjusted nominal speed for this block in (mm/min)^2 - float entry_speed_sqr; // Entry speed at previous-current block junction in (mm/min)^2 - float max_entry_speed_sqr; // Maximum allowable junction entry speed in (mm/min)^2 - float acceleration; // Axes-limit adjusted line acceleration in mm/min^2 - float millimeters; // The total travel for this block to be executed in mm + float acceleration; // Axis-limit adjusted line acceleration in mm/min^2 + float millimeters; // The remaining distance for this block to be executed in mm - // Settings for the trapezoid generator -// int32_t decelerate_after; // The index of the step event on which to start decelerating - } plan_block_t; // Initialize the motion plan subsystem @@ -66,7 +66,9 @@ plan_block_t *plan_get_current_block(); plan_block_t *plan_get_block_by_index(uint8_t block_index); -int32_t calculate_trapezoid_for_block(uint8_t block_index); +float plan_calculate_velocity_profile(uint8_t block_index); + +// void plan_update_partial_block(uint8_t block_index, float millimeters_remaining, uint8_t is_decelerating); // Reset the planner position vector (in steps) void plan_sync_position(); diff --git a/serial.c b/serial.c index 69fa717..d3325c7 100644 --- a/serial.c +++ b/serial.c @@ -91,11 +91,7 @@ void serial_write(uint8_t data) { } // Data Register Empty Interrupt handler -#ifdef __AVR_ATmega644P__ -ISR(USART0_UDRE_vect) -#else -ISR(USART_UDRE_vect) -#endif +ISR(SERIAL_UDRE) { // Temporary tx_buffer_tail (to optimize for volatile) uint8_t tail = tx_buffer_tail; @@ -144,11 +140,7 @@ uint8_t serial_read() } } -#ifdef __AVR_ATmega644P__ -ISR(USART0_RX_vect) -#else -ISR(USART_RX_vect) -#endif +ISR(SERIAL_RX) { uint8_t data = UDR0; uint8_t next_head; diff --git a/stepper.c b/stepper.c index 05ddd9e..6509bed 100644 --- a/stepper.c +++ b/stepper.c @@ -42,7 +42,7 @@ #define ST_DECEL 2 #define ST_DECEL_EOB 3 -#define SEGMENT_BUFFER_SIZE 10 +#define SEGMENT_BUFFER_SIZE 6 // Stepper state variable. Contains running data and trapezoid variables. typedef struct { @@ -50,11 +50,11 @@ typedef struct { int32_t counter_x, // Counter variables for the bresenham line tracer counter_y, counter_z; - uint8_t segment_steps_remaining; // Steps remaining in line motion + uint8_t segment_steps_remaining; // Steps remaining in line segment motion // Used by inverse time algorithm to track step rate - int32_t counter_d; // Inverse time distance traveled since last step event - uint32_t delta_d; // Inverse time distance traveled per interrupt tick + int32_t counter_d; // Inverse time distance traveled since last step event + uint32_t delta_d; // Inverse time distance traveled per interrupt tick uint32_t d_per_tick; // Used by the stepper driver interrupt @@ -68,9 +68,11 @@ typedef struct { } stepper_t; static stepper_t st; -// Stores stepper buffer common data. Can change planner mid-block in special conditions. +// Stores stepper buffer common data for a planner block. Data can change mid-block when the planner +// updates the remaining block velocity profile with a more optimal plan or a feedrate override occurs. +// NOTE: Normally, this buffer is only partially used, but can fill up completely in certain conditions. typedef struct { - int32_t step_events_remaining; // Tracks step event count for the executing planner block + int32_t step_events_remaining; // Tracks step event count for the executing planner block uint32_t d_next; // Scaled distance to next step uint32_t initial_rate; // Initialized step rate at re/start of a planner block uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute @@ -80,16 +82,17 @@ typedef struct { } st_data_t; static st_data_t segment_data[SEGMENT_BUFFER_SIZE]; -// Primary stepper motion buffer +// Primary stepper buffer. Contains small, short line segments for the stepper algorithm to execute checked +// out incrementally from the first block in the planner buffer. These step segments typedef struct { - uint8_t n_step; - uint8_t st_data_index; - uint8_t flag; + uint8_t n_step; // Number of step events to be executed for this segment + uint8_t st_data_index; // Stepper buffer common data index. Uses this information to execute this segment. + uint8_t flag; // Stepper algorithm execution flag to notify special conditions. } st_segment_t; static st_segment_t segment_buffer[SEGMENT_BUFFER_SIZE]; static volatile uint8_t segment_buffer_tail; -static uint8_t segment_buffer_head; +static volatile uint8_t segment_buffer_head; static uint8_t segment_next_head; static volatile uint8_t busy; // Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though. @@ -97,11 +100,16 @@ static plan_block_t *pl_current_block; // A pointer to the planner block curren static st_segment_t *st_current_segment; static st_data_t *st_current_data; +// Pointers for the step segment being prepped from the planner buffer. Accessed only by the +// main program. Pointers may be planning segments or planner blocks ahead of what being executed. static plan_block_t *pl_prep_block; // A pointer to the planner block being prepped into the stepper buffer static uint8_t pl_prep_index; static st_data_t *st_prep_data; static uint8_t st_data_prep_index; +static uint8_t pl_partial_block_flag; + + // Returns the index of the next block in the ring buffer // NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication. @@ -115,7 +123,7 @@ static uint8_t next_block_index(uint8_t block_index) static uint8_t next_block_pl_index(uint8_t block_index) { block_index++; - if (block_index == 18) { block_index = 0; } + if (block_index == BLOCK_BUFFER_SIZE) { block_index = 0; } return(block_index); } @@ -255,22 +263,20 @@ ISR(TIMER2_COMPA_vect) // Initialize inverse time and step rate counter data st.counter_d = st_current_data->d_next; // d_next always greater than delta_d. + if (st.delta_d < MINIMUM_STEP_RATE) { st.d_per_tick = MINIMUM_STEP_RATE; } + else { st.d_per_tick = st.delta_d; } // During feed hold, do not update rate or ramp type. Keep decelerating. // if (sys.state == STATE_CYCLE) { st.delta_d = st_current_data->initial_rate; -// if (st.delta_d == st_current_data->nominal_rate) { -// st.ramp_type = RAMP_NOOP_CRUISE; - st.ramp_type = RAMP_ACCEL; st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid -// } + if (st.delta_d == st_current_data->nominal_rate) { st.ramp_type = RAMP_NOOP_CRUISE; } + else { st.ramp_type = RAMP_ACCEL; } // } - if (st.delta_d < MINIMUM_STEP_RATE) { st.d_per_tick = MINIMUM_STEP_RATE; } - else { st.d_per_tick = st.delta_d; } } - // Acceleration and cruise handled by ramping. Just check for deceleration. + // Acceleration and cruise handled by ramping. Just check if deceleration needs to begin. if (st_current_segment->flag == ST_DECEL || st_current_segment->flag == ST_DECEL_EOB) { if (st.ramp_type == RAMP_NOOP_CRUISE) { st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid @@ -292,6 +298,8 @@ ISR(TIMER2_COMPA_vect) } // Adjust inverse time counter for ac/de-celerations + // NOTE: Accelerations are handled by the stepper algorithm as it's thought to be more computationally + // efficient on the Arduino AVR. This could change eventually, but it definitely will with ARM development. if (st.ramp_type) { st.ramp_count--; // Tick acceleration ramp counter if (st.ramp_count == 0) { // Adjust step rate when its time @@ -396,14 +404,20 @@ ISR(TIMER0_OVF_vect) void st_reset() { memset(&st, 0, sizeof(st)); - pl_current_block = NULL; - pl_prep_block = NULL; - pl_prep_index = 0; - st_data_prep_index = 0; + st.load_flag = LOAD_BLOCK; busy = false; + + pl_current_block = NULL; // Planner block pointer used by stepper algorithm + pl_prep_block = NULL; // Planner block pointer used by segment buffer + pl_prep_index = 0; // Planner buffer indices are also reset to zero. + st_data_prep_index = 0; + segment_buffer_tail = 0; + segment_buffer_head = 0; // empty = tail segment_next_head = 1; + + pl_partial_block_flag = false; } @@ -485,7 +499,7 @@ void st_cycle_reinitialize() } -/* Preps stepper buffer. Called from main program. +/* Prepares step segment buffer. Continuously called from main program. NOTE: There doesn't seem to be a great way to figure out how many steps occur within a set number of ISR ticks. Numerical round-off and CPU overhead always seems to be a @@ -510,41 +524,75 @@ void st_cycle_reinitialize() been doing here limit this phase issue by truncating some of the ramp timing for certain events like deceleration initialization and end of block. */ + +// !!! Need to make sure when a single partially completed block can be re-computed here with +// new deceleration point and the segment manager begins accelerating again immediately. void st_prep_buffer() { while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer. // Determine if we need to load a new planner block. if (pl_prep_block == NULL) { - pl_prep_block = plan_get_block_by_index(pl_prep_index); - if (pl_prep_block == NULL) { return; } // No more planner blocks. Let stepper finish out. + pl_prep_block = plan_get_block_by_index(pl_prep_index); // Query planner for a queued block + if (pl_prep_block == NULL) { return; } // No planner blocks. Exit. - // Prepare commonly shared planner block data for the ensuing step buffer moves - st_data_prep_index = next_block_index(st_data_prep_index); - st_prep_data = &segment_data[st_data_prep_index]; - - // Initialize Bresenham variables - st_prep_data->step_events_remaining = pl_prep_block->step_event_count; + // Check if the planner has re-computed this block mid-execution. If so, push the old segment block + // data Otherwise, prepare a new segment block data. + if (pl_partial_block_flag) { + + // Prepare new shared segment block data and copy the relevant last segment block data. + st_data_t *last_st_prep_data; + last_st_prep_data = &segment_data[st_data_prep_index]; + st_data_prep_index = next_block_index(st_data_prep_index); + st_prep_data = &segment_data[st_data_prep_index]; + + st_prep_data->step_events_remaining = last_st_prep_data->step_events_remaining; + st_prep_data->rate_delta = last_st_prep_data->rate_delta; + st_prep_data->d_next = last_st_prep_data->d_next; + st_prep_data->nominal_rate = last_st_prep_data->nominal_rate; // TODO: Recompute with feedrate overrides. + + st_prep_data->mm_per_step = last_st_prep_data->mm_per_step; - // Convert new block to stepper variables. - // NOTE: This data can change mid-block from normal planner updates and feedrate overrides. Must - // be maintained as these execute. - // TODO: If the planner updates this block, particularly from a deceleration to an acceleration, - // we must reload the initial rate data, such that the velocity profile is re-constructed correctly. - st_prep_data->initial_rate = ceil(sqrt(pl_prep_block->entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) - st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) - - // This data doesn't change. Could be performed in the planner, but fits nicely here. - // Although, acceleration can change for S-curves. So keep it here. - st_prep_data->rate_delta = ceil(pl_prep_block->acceleration* - ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) - // This definitely doesn't change, but could be precalculated in a way to help some of the - // math in this handler, i.e. millimeters per step event data. - st_prep_data->d_next = ceil((pl_prep_block->millimeters*INV_TIME_MULTIPLIER)/pl_prep_block->step_event_count); // (mult*mm/step) - st_prep_data->mm_per_step = pl_prep_block->millimeters/pl_prep_block->step_event_count; - - // Calculate trapezoid data from planner. - st_prep_data->decelerate_after = calculate_trapezoid_for_block(pl_prep_index); + pl_partial_block_flag = false; // Reset flag + + // TODO: If the planner updates this block, particularly from a deceleration to an acceleration, + // we must reload the initial rate data, such that the velocity profile is re-constructed correctly. + // The stepper algorithm must be flagged to adjust the acceleration counters. + + } else { + + // Prepare commonly shared planner block data for the ensuing segment buffer moves ad-hoc, since + // the planner buffer can dynamically change the velocity profile data as blocks are added. + st_data_prep_index = next_block_index(st_data_prep_index); + st_prep_data = &segment_data[st_data_prep_index]; + + // Initialize Bresenham variables + st_prep_data->step_events_remaining = pl_prep_block->step_event_count; + + // Convert planner block velocity profile data to stepper rate and step distance data. + st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + st_prep_data->rate_delta = ceil(pl_prep_block->acceleration* + ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) + st_prep_data->d_next = ceil((pl_prep_block->millimeters*INV_TIME_MULTIPLIER)/pl_prep_block->step_event_count); // (mult*mm/step) + + // TODO: Check if we really need to store this. + st_prep_data->mm_per_step = pl_prep_block->millimeters/pl_prep_block->step_event_count; + + } + + // Convert planner entry speed to stepper initial rate. + st_prep_data->initial_rate = ceil(sqrt(pl_prep_block->entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + + // TODO: Nominal rate changes with feedrate override. + // st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + + // Calculate the planner block velocity profile type and determine deceleration point. + float mm_decelerate_after = plan_calculate_velocity_profile(pl_prep_index); + if (mm_decelerate_after == pl_prep_block->millimeters) { + st_prep_data->decelerate_after = st_prep_data->step_events_remaining; + } else { + st_prep_data->decelerate_after = ceil( mm_decelerate_after/st_prep_data->mm_per_step ); + } } @@ -564,77 +612,108 @@ void st_prep_buffer() - From deceleration to acceleration, i.e. common with jogging when new blocks are added. */ - st_segment_t *st_prep_segment = &segment_buffer[segment_buffer_head]; - st_prep_segment->st_data_index = st_data_prep_index; + st_segment_t *new_segment = &segment_buffer[segment_buffer_head]; + new_segment->st_data_index = st_data_prep_index; // TODO: How do you cheaply compute n_step without a sqrt()? Could be performed as 'bins'. - st_prep_segment->n_step = 250; //floor( (exit_speed*approx_time)/mm_per_step ); -// st_segment->n_step = max(st_segment->n_step,MINIMUM_STEPS_PER_BLOCK); // Ensure it moves for very slow motions? -// st_segment->n_step = min(st_segment->n_step,MAXIMUM_STEPS_PER_BLOCK); // Prevent unsigned int8 overflow. + // The basic equation is: s = u*t + 0.5*a*t^2 + // For the most part, we can store the acceleration portion in the st_data buffer and all + // we would need to do is track the current approximate speed per loop with: v = u + a*t + // Each loop would require 3 multiplication and 2 additions, since most of the variables + // are constants and would get compiled out. + +//!!! Doesn't work as is. Requires last_velocity and acceleration in terms of steps, not mm. +// new_segment->n_step = ceil(last_velocity*TIME_PER_SEGMENT/mm_per_step); +// if (st_prep_data->decelerate_after > 0) { +// new_segment->n_step += ceil(pl_prep_block->acceleration*(0.5*TIME_PER_SEGMENT*TIME_PER_SEGMENT/(60*60))/mm_per_step); +// } else { +// new_segment->n_step -= ceil(pl_prep_block->acceleration*(0.5*TIME_PER_SEGMENT*TIME_PER_SEGMENT/(60*60))/mm_per_step); +// } + + new_segment->n_step = 7; //floor( (exit_speed*approx_time)/mm_per_step ); +// new_segment->n_step = max(new_segment->n_step,MINIMUM_STEPS_PER_BLOCK); // Ensure it moves for very slow motions? +// new_segment->n_step = min(new_segment->n_step,MAXIMUM_STEPS_PER_BLOCK); // Prevent unsigned int8 overflow. + // Check if n_step exceeds steps remaining in planner block. If so, truncate. - if (st_prep_segment->n_step > st_prep_data->step_events_remaining) { - st_prep_segment->n_step = st_prep_data->step_events_remaining; + if (new_segment->n_step > st_prep_data->step_events_remaining) { + new_segment->n_step = st_prep_data->step_events_remaining; + + // Don't need to compute last velocity, since it will be refreshed with a new block. } // Check if n_step exceeds decelerate point in block. Need to perform this so that the // ramp counters are reset correctly in the stepper algorithm. Can be 1 step, but should // be OK since it is likely moving at a fast rate already. if (st_prep_data->decelerate_after > 0) { - if (st_prep_segment->n_step > st_prep_data->decelerate_after) { - st_prep_segment->n_step = st_prep_data->decelerate_after; - } - } - -// float distance, exit_speed_sqr; -// distance = st_prep_segment->n_step*st_prep_data->mm_per_step; // Always greater than zero -// if (st_prep_data->step_events_remaining >= pl_prep_block->decelerate_after) { -// exit_speed_sqr = pl_prep_block->entry_speed_sqr - 2*pl_prep_block->acceleration*distance; -// } else { // Acceleration or cruising ramp -// if (pl_prep_block->entry_speed_sqr < pl_prep_block->nominal_speed_sqr) { -// exit_speed_sqr = pl_prep_block->entry_speed_sqr + 2*pl_prep_block->acceleration*distance; -// if (exit_speed_sqr > pl_prep_block->nominal_speed_sqr) { exit_speed_sqr = pl_prep_block->nominal_speed_sqr; } -// } else { -// exit_speed_sqr = pl_prep_block->nominal_speed_sqr; + if (new_segment->n_step > st_prep_data->decelerate_after) { + new_segment->n_step = st_prep_data->decelerate_after; + } +// !!! Doesn't work. Remove if not using. +// if (last_velocity < last_nominal_v) { +// // !!! Doesn't work since distance changes and gets truncated. +// last_velocity += pl_prep_block->acceleration*(TIME_PER_SEGMENT/(60*60)); // In acceleration ramp. +// if {last_velocity > last_nominal_v) { last_velocity = last_nominal_v; } // Set to cruising. // } -// } - - // Update planner block variables. -// pl_prep_block->entry_speed_sqr = max(0.0,exit_speed_sqr); -// pl_prep_block->max_entry_speed_sqr = exit_speed_sqr; // ??? Overwrites the corner speed. May need separate variable. -// pl_prep_block->millimeters -= distance; // Potential round-off error near end of block. -// pl_prep_block->millimeters = max(0.0,pl_prep_block->millimeters); // Shouldn't matter. +// } else { // In deceleration ramp +// last_velocity -= pl_prep_block->acceleration*(TIME_PER_SEGMENT/(60*60)); + } // Update stepper block variables. - st_prep_data->step_events_remaining -= st_prep_segment->n_step; + st_prep_data->step_events_remaining -= new_segment->n_step; if ( st_prep_data->step_events_remaining == 0 ) { // Move planner pointer to next block if (st_prep_data->decelerate_after == 0) { - st_prep_segment->flag = ST_DECEL_EOB; + new_segment->flag = ST_DECEL_EOB; // Flag when deceleration begins and ends at EOB. Could rewrite to use bit flags too. } else { - st_prep_segment->flag = ST_END_OF_BLOCK; + new_segment->flag = ST_END_OF_BLOCK; } pl_prep_index = next_block_pl_index(pl_prep_index); pl_prep_block = NULL; -printString("EOB"); } else { + // Current segment is mid-planner block. Just set the DECEL/NOOP acceleration flags. if (st_prep_data->decelerate_after == 0) { - st_prep_segment->flag = ST_DECEL; + new_segment->flag = ST_DECEL; } else { - st_prep_segment->flag = ST_NOOP; + new_segment->flag = ST_NOOP; } -printString("x"); + st_prep_data->decelerate_after -= new_segment->n_step; } - st_prep_data->decelerate_after -= st_prep_segment->n_step; + - // New step block completed. Increment step buffer indices. + // New step segment completed. Increment segment buffer indices. segment_buffer_head = segment_next_head; segment_next_head = next_block_index(segment_buffer_head); -printInteger((long)st_prep_segment->n_step); -printString(" "); -printInteger((long)st_prep_data->decelerate_after); -printString(" "); -printInteger((long)st_prep_data->step_events_remaining); } } + +uint8_t st_get_prep_block_index() +{ +// Returns only the index but doesn't state if the block has been partially executed. How do we simply check for this? + return(pl_prep_index); +} + +void st_fetch_partial_block_parameters(uint8_t block_index, float *millimeters_remaining, uint8_t *is_decelerating) +{ + // if called, can we assume that this always changes and needs to be updated? if so, then + // we can perform all of the segment buffer setup tasks here to make sure the next time + // the segments are loaded, the st_data buffer is updated correctly. + // !!! Make sure that this is always pointing to the correct st_prep_data block. + + // When a mid-block acceleration occurs, we have to make sure the ramp counters are updated + // correctly, much in the same fashion as the deceleration counters. Need to think about this + // make sure this is right, but i'm pretty sure it is. + + // TODO: NULL means that the segment buffer has completed the block. Need to clean this up a bit. + if (pl_prep_block != NULL) { + *millimeters_remaining = st_prep_data->step_events_remaining*st_prep_data->mm_per_step; + if (st_prep_data->decelerate_after > 0) { *is_decelerating = false; } + else { *is_decelerating = true; } + + // Flag for new prep_block when st_prep_buffer() is called after the planner recomputes. + pl_partial_block_flag = true; + pl_prep_block = NULL; + } + return; +} diff --git a/stepper.h b/stepper.h index 74ab717..8e2a6e0 100644 --- a/stepper.h +++ b/stepper.h @@ -47,4 +47,8 @@ void st_feed_hold(); void st_prep_buffer(); +uint8_t st_get_prep_block_index(); + +void st_fetch_partial_block_parameters(uint8_t block_index, float *millimeters_remaining, uint8_t *is_decelerating); + #endif From 8a10654b1cb694f4b24f7fd247acc345f3275d28 Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Sat, 12 Oct 2013 10:35:26 -0600 Subject: [PATCH 14/73] New stepper subsystem bug fixes. - New stepper algorithm with the new optimized planner seems to be working nearly twice as fast as the previous algorithm. - For one, the planner computation overhead is probably a fraction of what it used to be with the worst case being about half still. - Secondly, anytime the planner plans back to the first executing block, it no longer overwrites the block conditions and allows it to complete without lost steps. So no matter if the streams slows, the protected planner should keep the steppers moving without risk of lost steps (although this still needs to be tested thoroughly and may audibly sound weird when this happens.) - It now seems that the bottleneck is the serial baudrate (which is good!) --- stepper.c | 173 ++++++++++++++++++++++++++++++++---------------------- 1 file changed, 104 insertions(+), 69 deletions(-) diff --git a/stepper.c b/stepper.c index 6509bed..aab0be4 100644 --- a/stepper.c +++ b/stepper.c @@ -34,13 +34,12 @@ #define RAMP_DECEL 2 #define LOAD_NOOP 0 -#define LOAD_LINE 1 +#define LOAD_SEGMENT 1 #define LOAD_BLOCK 2 -#define ST_NOOP 0 -#define ST_END_OF_BLOCK 1 -#define ST_DECEL 2 -#define ST_DECEL_EOB 3 +#define ST_END_OF_BLOCK bit(0) +#define ST_ACCEL bit(1) +#define ST_DECEL bit(2) #define SEGMENT_BUFFER_SIZE 6 @@ -87,7 +86,7 @@ static st_data_t segment_data[SEGMENT_BUFFER_SIZE]; typedef struct { uint8_t n_step; // Number of step events to be executed for this segment uint8_t st_data_index; // Stepper buffer common data index. Uses this information to execute this segment. - uint8_t flag; // Stepper algorithm execution flag to notify special conditions. + uint8_t flag; // Stepper algorithm bit-flag for special execution conditions. } st_segment_t; static st_segment_t segment_buffer[SEGMENT_BUFFER_SIZE]; @@ -208,8 +207,17 @@ void st_go_idle() stepper port after each pulse. The bresenham line tracer algorithm controls all three stepper outputs simultaneously with these two interrupts. */ -// NOTE: Average time in this ISR is: 5 usec iterating timers only, 20-25 usec with step event, or -// 15 usec when popping a block. So, ensure Ranade frequency and step pulse times work with this. +/* TODO: + - Measure time in ISR. Typical and worst-case. + - Write how the acceleration counters work and why they are set at half via mid-point rule. + - Determine if placing the position counters elsewhere (or change them to 8-bit variables that + are added to the system position counters at the end of a segment) frees up cycles. + - Write a blurb about how the acceleration should be handled within the ISR. All of the + time/step/ramp counters accurately keep track of the remainders and phasing of the variables + with time. This means we do not have to compute them via expensive floating point beforehand. + - Need to do an analysis to determine if these counters are really that much cheaper. At least + find out when it isn't anymore. Particularly when the ISR is at a very high frequency. +*/ ISR(TIMER2_COMPA_vect) { // SPINDLE_ENABLE_PORT ^= 1<initial_rate; - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Initialize ramp counter via midpoint rule if (st.delta_d == st_current_data->nominal_rate) { st.ramp_type = RAMP_NOOP_CRUISE; } else { st.ramp_type = RAMP_ACCEL; } // } @@ -277,16 +285,20 @@ ISR(TIMER2_COMPA_vect) } // Acceleration and cruise handled by ramping. Just check if deceleration needs to begin. - if (st_current_segment->flag == ST_DECEL || st_current_segment->flag == ST_DECEL_EOB) { - if (st.ramp_type == RAMP_NOOP_CRUISE) { - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid - } else { - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; // Set ramp counter for triangle - } - st.ramp_type = RAMP_DECEL; + if ( st_current_segment->flag & (ST_DECEL | ST_ACCEL) ) { + /* Compute correct ramp count for a ramp change. Upon a switch from acceleration to deceleration, + or vice-versa, the new ramp count must be set to trigger the next acceleration tick equal to + the number of ramp ISR ticks counted since the last acceleration tick. This is ensures the + ramp is executed exactly as the plan dictates. Otherwise, when a ramp begins from a known + rate (nominal/cruise or initial), the ramp count must be set to ISR_TICKS_PER_ACCELERATION_TICK/2 + as mandated by the mid-point rule. For these conditions, the ramp count has been pre-initialized + such that the following computation is still correct. */ + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; + if ( st_current_segment->flag & ST_DECEL ) { st.ramp_type = RAMP_DECEL; } + else { st.ramp_type = RAMP_ACCEL; } } - st.load_flag = LOAD_NOOP; // Motion loaded. Set no-operation flag until complete. + st.load_flag = LOAD_NOOP; // Segment motion loaded. Set no-operation flag to skip during execution. } else { // Can't discard planner block here if a feed hold stops in middle of block. @@ -299,18 +311,20 @@ ISR(TIMER2_COMPA_vect) // Adjust inverse time counter for ac/de-celerations // NOTE: Accelerations are handled by the stepper algorithm as it's thought to be more computationally - // efficient on the Arduino AVR. This could change eventually, but it definitely will with ARM development. - if (st.ramp_type) { + // efficient on the Arduino AVR. This could may not be true with higher ISR frequencies or faster CPUs. + if (st.ramp_type) { // Ignored when ramp type is NOOP_CRUISE st.ramp_count--; // Tick acceleration ramp counter if (st.ramp_count == 0) { // Adjust step rate when its time - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter if (st.ramp_type == RAMP_ACCEL) { // Adjust velocity for acceleration + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter st.delta_d += st_current_data->rate_delta; if (st.delta_d >= st_current_data->nominal_rate) { // Reached nominal rate. st.delta_d = st_current_data->nominal_rate; // Set cruising velocity - st.ramp_type = RAMP_NOOP_CRUISE; // Set ramp flag to ignore + st.ramp_type = RAMP_NOOP_CRUISE; // Set ramp flag to cruising + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Re-initialize counter for next ramp. } - } else { // Adjust velocity for deceleration + } else { // Adjust velocity for deceleration. + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter if (st.delta_d > st_current_data->rate_delta) { st.delta_d -= st_current_data->rate_delta; } else { @@ -318,7 +332,11 @@ ISR(TIMER2_COMPA_vect) // Moving near zero feed rate. Gracefully slow down. st.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow. - // Check for and handle feed hold exit? At this point, machine is stopped. + // TODO: Check for and handle feed hold exit? At this point, machine is stopped. + // - Set system flag to recompute plan and reset segment buffer. + // - Segment steps in buffer needs to be returned to planner correctly. + // busy = false; + // return; } } @@ -343,6 +361,7 @@ ISR(TIMER2_COMPA_vect) if (st.counter_x < 0) { st.out_bits |= (1<step_event_count; + // st.steps_x++; if (st.out_bits & (1<step_event_count; + // st.steps_y++; if (st.out_bits & (1<step_event_count; + // st.steps_z++; if (st.out_bits & (1< 0) { + if (st.out_bits & (1< 0) { + if (st.out_bits & (1< 0) { + if (st.out_bits & (1<flag == ST_END_OF_BLOCK || st_current_segment->flag == ST_DECEL_EOB) { + if (st_current_segment->flag & ST_END_OF_BLOCK) { plan_discard_current_block(); st.load_flag = LOAD_BLOCK; } else { - st.load_flag = LOAD_LINE; + st.load_flag = LOAD_SEGMENT; } // Discard current segment segment_buffer_tail = next_block_index( segment_buffer_tail ); - - // NOTE: sys.position updates could be done here. The bresenham counters can have - // their own fast 8-bit addition-only counters. Here we would check the direction and - // apply it to sys.position accordingly. However, this could take too much time. - + } st.out_bits ^= settings.invert_mask; // Apply step port invert mask @@ -531,13 +575,17 @@ void st_prep_buffer() { while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer. + + st_segment_t *prep_segment = &segment_buffer[segment_buffer_head]; + prep_segment->flag = 0; + // Determine if we need to load a new planner block. if (pl_prep_block == NULL) { pl_prep_block = plan_get_block_by_index(pl_prep_index); // Query planner for a queued block if (pl_prep_block == NULL) { return; } // No planner blocks. Exit. - + // Check if the planner has re-computed this block mid-execution. If so, push the old segment block - // data Otherwise, prepare a new segment block data. + // data. Otherwise, prepare a new segment block data for the new planner block. if (pl_partial_block_flag) { // Prepare new shared segment block data and copy the relevant last segment block data. @@ -553,6 +601,8 @@ void st_prep_buffer() st_prep_data->mm_per_step = last_st_prep_data->mm_per_step; + prep_segment->flag |= ST_ACCEL; + pl_partial_block_flag = false; // Reset flag // TODO: If the planner updates this block, particularly from a deceleration to an acceleration, @@ -588,11 +638,7 @@ void st_prep_buffer() // Calculate the planner block velocity profile type and determine deceleration point. float mm_decelerate_after = plan_calculate_velocity_profile(pl_prep_index); - if (mm_decelerate_after == pl_prep_block->millimeters) { - st_prep_data->decelerate_after = st_prep_data->step_events_remaining; - } else { - st_prep_data->decelerate_after = ceil( mm_decelerate_after/st_prep_data->mm_per_step ); - } + st_prep_data->decelerate_after = ceil( mm_decelerate_after/st_prep_data->mm_per_step ); } @@ -612,8 +658,7 @@ void st_prep_buffer() - From deceleration to acceleration, i.e. common with jogging when new blocks are added. */ - st_segment_t *new_segment = &segment_buffer[segment_buffer_head]; - new_segment->st_data_index = st_data_prep_index; + prep_segment->st_data_index = st_data_prep_index; // TODO: How do you cheaply compute n_step without a sqrt()? Could be performed as 'bins'. // The basic equation is: s = u*t + 0.5*a*t^2 @@ -623,21 +668,21 @@ void st_prep_buffer() // are constants and would get compiled out. //!!! Doesn't work as is. Requires last_velocity and acceleration in terms of steps, not mm. -// new_segment->n_step = ceil(last_velocity*TIME_PER_SEGMENT/mm_per_step); +// prep_segment->n_step = ceil(last_velocity*TIME_PER_SEGMENT/mm_per_step); // if (st_prep_data->decelerate_after > 0) { -// new_segment->n_step += ceil(pl_prep_block->acceleration*(0.5*TIME_PER_SEGMENT*TIME_PER_SEGMENT/(60*60))/mm_per_step); +// prep_segment->n_step += ceil(pl_prep_block->acceleration*(0.5*TIME_PER_SEGMENT*TIME_PER_SEGMENT/(60*60))/mm_per_step); // } else { -// new_segment->n_step -= ceil(pl_prep_block->acceleration*(0.5*TIME_PER_SEGMENT*TIME_PER_SEGMENT/(60*60))/mm_per_step); +// prep_segment->n_step -= ceil(pl_prep_block->acceleration*(0.5*TIME_PER_SEGMENT*TIME_PER_SEGMENT/(60*60))/mm_per_step); // } - new_segment->n_step = 7; //floor( (exit_speed*approx_time)/mm_per_step ); -// new_segment->n_step = max(new_segment->n_step,MINIMUM_STEPS_PER_BLOCK); // Ensure it moves for very slow motions? -// new_segment->n_step = min(new_segment->n_step,MAXIMUM_STEPS_PER_BLOCK); // Prevent unsigned int8 overflow. + prep_segment->n_step = 15; //floor( (exit_speed*approx_time)/mm_per_step ); +// prep_segment->n_step = max(prep_segment->n_step,MINIMUM_STEPS_PER_BLOCK); // Ensure it moves for very slow motions? +// prep_segment->n_step = min(prep_segment->n_step,MAXIMUM_STEPS_PER_BLOCK); // Prevent unsigned int8 overflow. // Check if n_step exceeds steps remaining in planner block. If so, truncate. - if (new_segment->n_step > st_prep_data->step_events_remaining) { - new_segment->n_step = st_prep_data->step_events_remaining; + if (prep_segment->n_step > st_prep_data->step_events_remaining) { + prep_segment->n_step = st_prep_data->step_events_remaining; // Don't need to compute last velocity, since it will be refreshed with a new block. } @@ -646,8 +691,8 @@ void st_prep_buffer() // ramp counters are reset correctly in the stepper algorithm. Can be 1 step, but should // be OK since it is likely moving at a fast rate already. if (st_prep_data->decelerate_after > 0) { - if (new_segment->n_step > st_prep_data->decelerate_after) { - new_segment->n_step = st_prep_data->decelerate_after; + if (prep_segment->n_step > st_prep_data->decelerate_after) { + prep_segment->n_step = st_prep_data->decelerate_after; } // !!! Doesn't work. Remove if not using. // if (last_velocity < last_nominal_v) { @@ -657,29 +702,19 @@ void st_prep_buffer() // } // } else { // In deceleration ramp // last_velocity -= pl_prep_block->acceleration*(TIME_PER_SEGMENT/(60*60)); + } else { + if (st_prep_data->decelerate_after == 0) { prep_segment->flag |= ST_DECEL; } } - + st_prep_data->decelerate_after -= prep_segment->n_step; + // Update stepper block variables. - st_prep_data->step_events_remaining -= new_segment->n_step; + st_prep_data->step_events_remaining -= prep_segment->n_step; if ( st_prep_data->step_events_remaining == 0 ) { - // Move planner pointer to next block - if (st_prep_data->decelerate_after == 0) { - new_segment->flag = ST_DECEL_EOB; // Flag when deceleration begins and ends at EOB. Could rewrite to use bit flags too. - } else { - new_segment->flag = ST_END_OF_BLOCK; - } + prep_segment->flag |= ST_END_OF_BLOCK; + // Move planner pointer to next block and flag to load a new block for the next segment. pl_prep_index = next_block_pl_index(pl_prep_index); pl_prep_block = NULL; - } else { - // Current segment is mid-planner block. Just set the DECEL/NOOP acceleration flags. - if (st_prep_data->decelerate_after == 0) { - new_segment->flag = ST_DECEL; - } else { - new_segment->flag = ST_NOOP; - } - st_prep_data->decelerate_after -= new_segment->n_step; - } - + } // New step segment completed. Increment segment buffer indices. segment_buffer_head = segment_next_head; @@ -705,7 +740,7 @@ void st_fetch_partial_block_parameters(uint8_t block_index, float *millimeters_r // correctly, much in the same fashion as the deceleration counters. Need to think about this // make sure this is right, but i'm pretty sure it is. - // TODO: NULL means that the segment buffer has completed the block. Need to clean this up a bit. + // TODO: NULL means that the segment buffer has just completed a planner block. Clean up! if (pl_prep_block != NULL) { *millimeters_remaining = st_prep_data->step_events_remaining*st_prep_data->mm_per_step; if (st_prep_data->decelerate_after > 0) { *is_decelerating = false; } From 0cb5756b5310b8bf0cbc554393d14e86a0186c05 Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Mon, 14 Oct 2013 21:21:56 -0600 Subject: [PATCH 15/73] Fine tuning of new stepper algorithm with protected planner. Adaptive step prediction for segment buffer. - Cleaned up the new stepper algorithm code with more commenting and better logic flow. - The new segment buffer now predicts the number of steps each segment should have to execute over about 8 milliseconds each (based on the ACCELERATION_TICKS_PER_SECOND setting). So, for when the whole segment buffer is full, the stepper algorithm has roughly 40 milliseconds of steps queued before it needs to refilled by the main program. - Readjusted the max supported step rate back to 30kHz from the lower development 20kHz. Everything still works amazing great and the test CNC machine still runs twice as fast with the new stepper algorithm and planner. - Upped the standard serial baudrate to 115200 baud, as it is clear that the bottleneck is the serial interface. Will now support this, as well as the old 9600 baud, in new firmware builds. --- config.h | 4 +- defaults.h | 4 +- planner.c | 3 + stepper.c | 226 +++++++------- stepper_new2.c | 601 ------------------------------------ stepper_new_dual_ISR.c | 425 -------------------------- stepper_new_time.c | 680 ----------------------------------------- 7 files changed, 122 insertions(+), 1821 deletions(-) delete mode 100644 stepper_new2.c delete mode 100644 stepper_new_dual_ISR.c delete mode 100644 stepper_new_time.c diff --git a/config.h b/config.h index 41d6a38..f8f0810 100644 --- a/config.h +++ b/config.h @@ -57,7 +57,7 @@ // interrupt of the current stepper driver algorithm theoretically up to a frequency of 35-40kHz, but // CPU overhead increases exponentially as this frequency goes up. So there will be little left for // other processes like arcs. -#define ISR_TICKS_PER_SECOND 20000L // Integer (Hz) +#define ISR_TICKS_PER_SECOND 30000L // 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 @@ -66,7 +66,7 @@ // 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. // NOTE: Ramp count variable type in stepper module may need to be updated if changed. -#define ACCELERATION_TICKS_PER_SECOND 100L +#define ACCELERATION_TICKS_PER_SECOND 120L // NOTE: Make sure this value is less than 256, when adjusting both dependent parameters. #define ISR_TICKS_PER_ACCELERATION_TICK (ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND) diff --git a/defaults.h b/defaults.h index df3903c..d3ddc1c 100644 --- a/defaults.h +++ b/defaults.h @@ -140,9 +140,9 @@ #define DEFAULT_Z_STEPS_PER_MM (STEPS_PER_REV*MICROSTEPS/MM_PER_REV) #define DEFAULT_STEP_PULSE_MICROSECONDS 10 #define DEFAULT_ARC_TOLERANCE 0.005 // mm - #define DEFAULT_RAPID_FEEDRATE 2500.0 // mm/min + #define DEFAULT_RAPID_FEEDRATE 4000.0 // mm/min #define DEFAULT_FEEDRATE 1000.0 // mm/min - #define DEFAULT_ACCELERATION 150.0*60*60 // 150 mm/min^2 + #define DEFAULT_ACCELERATION 400.0*60*60 // 150 mm/min^2 #define DEFAULT_JUNCTION_DEVIATION 0.05 // mm #define DEFAULT_STEPPING_INVERT_MASK (1<entry_speed_sqr < next->entry_speed_sqr) { entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters; // If true, current block is full-acceleration and we can move the planned pointer forward. diff --git a/stepper.c b/stepper.c index aab0be4..27cf7d6 100644 --- a/stepper.c +++ b/stepper.c @@ -37,9 +37,12 @@ #define LOAD_SEGMENT 1 #define LOAD_BLOCK 2 -#define ST_END_OF_BLOCK bit(0) -#define ST_ACCEL bit(1) -#define ST_DECEL bit(2) +#define SEGMENT_NOOP 0 +#define SEGMENT_END_OF_BLOCK bit(0) +#define SEGMENT_ACCEL bit(1) +#define SEGMENT_DECEL bit(2) + +#define MINIMUM_STEPS_PER_SEGMENT 1 // Don't change #define SEGMENT_BUFFER_SIZE 6 @@ -67,22 +70,25 @@ typedef struct { } stepper_t; static stepper_t st; -// Stores stepper buffer common data for a planner block. Data can change mid-block when the planner -// updates the remaining block velocity profile with a more optimal plan or a feedrate override occurs. -// NOTE: Normally, this buffer is only partially used, but can fill up completely in certain conditions. +// Stores stepper common data for executing steps in the segment buffer. Data can change mid-block when the +// planner updates the remaining block velocity profile with a more optimal plan or a feedrate override occurs. +// NOTE: Normally, this buffer is partially in-use, but, for the worst case scenario, it will never exceed +// the number of accessible stepper buffer segments (SEGMENT_BUFFER_SIZE-1). typedef struct { - int32_t step_events_remaining; // Tracks step event count for the executing planner block - uint32_t d_next; // Scaled distance to next step - uint32_t initial_rate; // Initialized step rate at re/start of a planner block - uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute - uint32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive) - int32_t decelerate_after; + int32_t step_events_remaining; // Tracks step event count for the executing planner block + uint32_t d_next; // Scaled distance to next step + uint32_t initial_rate; // Initialized step rate at re/start of a planner block + uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute + uint32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive) + uint32_t current_approx_rate; // Tracks the approximate segment rate to predict steps per segment to execute + int32_t decelerate_after; // Tracks when to initiate deceleration according to the planner block float mm_per_step; } st_data_t; -static st_data_t segment_data[SEGMENT_BUFFER_SIZE]; +static st_data_t segment_data[SEGMENT_BUFFER_SIZE-1]; -// Primary stepper buffer. Contains small, short line segments for the stepper algorithm to execute checked -// out incrementally from the first block in the planner buffer. These step segments +// Primary stepper segment ring buffer. Contains small, short line segments for the stepper algorithm to execute, +// which are "checked-out" incrementally from the first block in the planner buffer. Once "checked-out", the steps +// in the segments buffer cannot be modified by the planner, where the remaining planner block steps still can. typedef struct { uint8_t n_step; // Number of step events to be executed for this segment uint8_t st_data_index; // Stepper buffer common data index. Uses this information to execute this segment. @@ -90,6 +96,7 @@ typedef struct { } st_segment_t; static st_segment_t segment_buffer[SEGMENT_BUFFER_SIZE]; +// Step segment ring buffer indices static volatile uint8_t segment_buffer_tail; static volatile uint8_t segment_buffer_head; static uint8_t segment_next_head; @@ -101,13 +108,11 @@ static st_data_t *st_current_data; // Pointers for the step segment being prepped from the planner buffer. Accessed only by the // main program. Pointers may be planning segments or planner blocks ahead of what being executed. -static plan_block_t *pl_prep_block; // A pointer to the planner block being prepped into the stepper buffer -static uint8_t pl_prep_index; -static st_data_t *st_prep_data; -static uint8_t st_data_prep_index; - -static uint8_t pl_partial_block_flag; - +static plan_block_t *pl_prep_block; // Pointer to the planner block being prepped +static st_data_t *st_prep_data; // Pointer to the stepper common data being prepped +static uint8_t pl_prep_index; // Index of planner block being prepped +static uint8_t st_data_prep_index; // Index of stepper common data block being prepped +static uint8_t pl_partial_block_flag; // Flag indicating the planner has modified the prepped planner block // Returns the index of the next block in the ring buffer @@ -194,21 +199,24 @@ void st_go_idle() /* "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Grbl. It is based - on the Pramod Ranade inverse time stepper algorithm, where a timer ticks at a constant - frequency and uses time-distance counters to track when its the approximate time for any - step event. However, the Ranade algorithm, as described, is susceptible to numerical round-off, - meaning that some axes steps may not execute for a given multi-axis motion. - Grbl's algorithm slightly differs by using a single Ranade time-distance counter to manage - a Bresenham line algorithm for multi-axis step events which ensures the number of steps for - each axis are executed exactly. In other words, it uses a Bresenham within a Bresenham algorithm, - where one tracks time(Ranade) and the other steps. - This interrupt pops blocks from the block_buffer and executes them by pulsing the stepper pins - appropriately. It is supported by The Stepper Port Reset Interrupt which it uses to reset the - stepper port after each pulse. The bresenham line tracer algorithm controls all three stepper - outputs simultaneously with these two interrupts. + on an inverse time stepper algorithm, where a timer ticks at a constant frequency and uses + time-distance counters to track when its the approximate time for a step event. For reference, + a similar inverse-time algorithm by Pramod Ranade is susceptible to numerical round-off, + meaning that some axes steps may not execute correctly for a given multi-axis motion. + Grbl's algorithm differs by using a single inverse time-distance counter to manage a + Bresenham line algorithm for multi-axis step events, which ensures the number of steps for + each axis are executed exactly. In other words, Grbl uses a Bresenham within a Bresenham + algorithm, where one tracks time for step events and the other steps for multi-axis moves. + Grbl specifically uses the Bresenham algorithm due to its innate mathematical exactness and + low computational overhead, requiring simple integer +,- counters only. + This interrupt pops blocks from the step segment buffer and executes them by pulsing the + stepper pins appropriately. It is supported by The Stepper Port Reset Interrupt which it uses + to reset the stepper port after each pulse. The bresenham line tracer algorithm controls all + three stepper outputs simultaneously with these two interrupts. */ /* TODO: - - Measure time in ISR. Typical and worst-case. + - Measure time in ISR. Typical and worst-case. Should be virtually identical to last algorithm. + There are no major changes to the base operations of this ISR with the new segment buffer. - Write how the acceleration counters work and why they are set at half via mid-point rule. - Determine if placing the position counters elsewhere (or change them to 8-bit variables that are added to the system position counters at the end of a segment) frees up cycles. @@ -278,23 +286,22 @@ ISR(TIMER2_COMPA_vect) // if (sys.state == STATE_CYCLE) { st.delta_d = st_current_data->initial_rate; st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Initialize ramp counter via midpoint rule - if (st.delta_d == st_current_data->nominal_rate) { st.ramp_type = RAMP_NOOP_CRUISE; } - else { st.ramp_type = RAMP_ACCEL; } + st.ramp_type = RAMP_NOOP_CRUISE; // Initialize as no ramp operation. Corrected later if necessary. // } } // Acceleration and cruise handled by ramping. Just check if deceleration needs to begin. - if ( st_current_segment->flag & (ST_DECEL | ST_ACCEL) ) { + if ( st_current_segment->flag & (SEGMENT_DECEL | SEGMENT_ACCEL) ) { /* Compute correct ramp count for a ramp change. Upon a switch from acceleration to deceleration, or vice-versa, the new ramp count must be set to trigger the next acceleration tick equal to the number of ramp ISR ticks counted since the last acceleration tick. This is ensures the ramp is executed exactly as the plan dictates. Otherwise, when a ramp begins from a known rate (nominal/cruise or initial), the ramp count must be set to ISR_TICKS_PER_ACCELERATION_TICK/2 - as mandated by the mid-point rule. For these conditions, the ramp count has been pre-initialized + as mandated by the mid-point rule. For these conditions, the ramp count have been initialized such that the following computation is still correct. */ st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; - if ( st_current_segment->flag & ST_DECEL ) { st.ramp_type = RAMP_DECEL; } + if ( st_current_segment->flag & SEGMENT_DECEL ) { st.ramp_type = RAMP_DECEL; } else { st.ramp_type = RAMP_ACCEL; } } @@ -415,7 +422,7 @@ ISR(TIMER2_COMPA_vect) // Line move is complete, set load line flag to check for new move. // Check if last line move in planner block. Discard if so. - if (st_current_segment->flag & ST_END_OF_BLOCK) { + if (st_current_segment->flag & SEGMENT_END_OF_BLOCK) { plan_discard_current_block(); st.load_flag = LOAD_BLOCK; } else { @@ -434,8 +441,8 @@ ISR(TIMER2_COMPA_vect) } -// The Stepper Port Reset Interrupt: Timer0 OVF interrupt handles the falling edge of the -// step pulse. This should always trigger before the next Timer2 COMPA interrupt and independently +// The Stepper Port Reset Interrupt: Timer0 OVF interrupt handles the falling edge of the step +// pulse. This should always trigger before the next Timer2 COMPA interrupt and independently // finish, if Timer2 is disabled after completing a move. ISR(TIMER0_OVF_vect) { @@ -569,51 +576,61 @@ void st_cycle_reinitialize() events like deceleration initialization and end of block. */ -// !!! Need to make sure when a single partially completed block can be re-computed here with -// new deceleration point and the segment manager begins accelerating again immediately. +/* + TODO: Figure out how to enforce a deceleration when a feedrate override is reduced. + The problem is that when an override is reduced, the planner may not plan back to + the current rate. Meaning that the velocity profiles for certain conditions no longer + are trapezoidal or triangular. For example, if the current block is cruising at a + nominal rate and the feedrate override is reduced, the new nominal rate will now be + lower. The velocity profile must first decelerate to the new nominal rate and then + follow on the new plan. So the remaining velocity profile will have a decelerate, + cruise, and another decelerate. + Another issue is whether or not a feedrate override reduction causes a deceleration + that acts over several planner blocks. For example, say that the plan is already + heavily decelerating throughout it, reducing the feedrate will not do much to it. So, + how do we determine when to resume the new plan? How many blocks do we have to wait + until the new plan intersects with the deceleration curve? One plus though, the + deceleration will never be more than the number of blocks in the entire planner buffer, + but it theoretically can be equal to it when all planner blocks are decelerating already. +*/ void st_prep_buffer() { while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer. - + // Initialize new segment st_segment_t *prep_segment = &segment_buffer[segment_buffer_head]; - prep_segment->flag = 0; + prep_segment->flag = SEGMENT_NOOP; // Determine if we need to load a new planner block. if (pl_prep_block == NULL) { pl_prep_block = plan_get_block_by_index(pl_prep_index); // Query planner for a queued block if (pl_prep_block == NULL) { return; } // No planner blocks. Exit. - // Check if the planner has re-computed this block mid-execution. If so, push the old segment block - // data. Otherwise, prepare a new segment block data for the new planner block. + // Increment stepper common data buffer index + if ( ++st_data_prep_index == (SEGMENT_BUFFER_SIZE-1) ) { st_data_prep_index = 0; } + + // Check if the planner has re-computed this block mid-execution. If so, push the previous segment + // data. Otherwise, prepare a new segment data for the new planner block. if (pl_partial_block_flag) { // Prepare new shared segment block data and copy the relevant last segment block data. st_data_t *last_st_prep_data; - last_st_prep_data = &segment_data[st_data_prep_index]; - st_data_prep_index = next_block_index(st_data_prep_index); + last_st_prep_data = st_prep_data; st_prep_data = &segment_data[st_data_prep_index]; st_prep_data->step_events_remaining = last_st_prep_data->step_events_remaining; st_prep_data->rate_delta = last_st_prep_data->rate_delta; st_prep_data->d_next = last_st_prep_data->d_next; - st_prep_data->nominal_rate = last_st_prep_data->nominal_rate; // TODO: Recompute with feedrate overrides. + st_prep_data->nominal_rate = last_st_prep_data->nominal_rate; // TODO: Feedrate overrides recomputes this. st_prep_data->mm_per_step = last_st_prep_data->mm_per_step; - prep_segment->flag |= ST_ACCEL; - pl_partial_block_flag = false; // Reset flag - // TODO: If the planner updates this block, particularly from a deceleration to an acceleration, - // we must reload the initial rate data, such that the velocity profile is re-constructed correctly. - // The stepper algorithm must be flagged to adjust the acceleration counters. - } else { // Prepare commonly shared planner block data for the ensuing segment buffer moves ad-hoc, since // the planner buffer can dynamically change the velocity profile data as blocks are added. - st_data_prep_index = next_block_index(st_data_prep_index); st_prep_data = &segment_data[st_data_prep_index]; // Initialize Bresenham variables @@ -636,81 +653,64 @@ void st_prep_buffer() // TODO: Nominal rate changes with feedrate override. // st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) - // Calculate the planner block velocity profile type and determine deceleration point. + st_prep_data->current_approx_rate = st_prep_data->initial_rate; + + // Calculate the planner block velocity profile type, determine deceleration point, and initial ramp. float mm_decelerate_after = plan_calculate_velocity_profile(pl_prep_index); st_prep_data->decelerate_after = ceil( mm_decelerate_after/st_prep_data->mm_per_step ); - + if (st_prep_data->decelerate_after > 0) { // If 0, SEGMENT_DECEL flag is set later. + if (st_prep_data->initial_rate != st_prep_data->nominal_rate) { prep_segment->flag = SEGMENT_ACCEL; } + } } - - /* - TODO: Need to check for a planner flag to indicate a change to this planner block. - If so, need to check for a change in acceleration state, from deceleration to acceleration, - to reset the stepper ramp counters and the initial_rate data to trace the new - ac/de-celeration profile correctly. - No change conditions: - - From nominal speed to acceleration from feedrate override - - From nominal speed to new deceleration. - - From acceleration to new deceleration point later or cruising point. - - From acceleration to immediate deceleration? Can happen during feedrate override - and slowing down, but likely ok by enforcing the normal ramp counter protocol. - Change conditions: - - From deceleration to acceleration, i.e. common with jogging when new blocks are added. - */ - + // Set new segment to point to the current segment data block. prep_segment->st_data_index = st_data_prep_index; - // TODO: How do you cheaply compute n_step without a sqrt()? Could be performed as 'bins'. - // The basic equation is: s = u*t + 0.5*a*t^2 - // For the most part, we can store the acceleration portion in the st_data buffer and all - // we would need to do is track the current approximate speed per loop with: v = u + a*t - // Each loop would require 3 multiplication and 2 additions, since most of the variables - // are constants and would get compiled out. + // Approximate the velocity over the new segment + if (st_prep_data->decelerate_after <= 0) { + if (st_prep_data->decelerate_after == 0) { prep_segment->flag = SEGMENT_DECEL; } + else { st_prep_data->current_approx_rate -= st_prep_data->rate_delta; } + if (st_prep_data->current_approx_rate < st_prep_data->rate_delta) { st_prep_data->current_approx_rate >>= 1; } + } else { + if (st_prep_data->current_approx_rate < st_prep_data->nominal_rate) { + st_prep_data->current_approx_rate += st_prep_data->rate_delta; + if (st_prep_data->current_approx_rate > st_prep_data->nominal_rate) { + st_prep_data->current_approx_rate = st_prep_data->nominal_rate; + } + } + } -//!!! Doesn't work as is. Requires last_velocity and acceleration in terms of steps, not mm. -// prep_segment->n_step = ceil(last_velocity*TIME_PER_SEGMENT/mm_per_step); -// if (st_prep_data->decelerate_after > 0) { -// prep_segment->n_step += ceil(pl_prep_block->acceleration*(0.5*TIME_PER_SEGMENT*TIME_PER_SEGMENT/(60*60))/mm_per_step); -// } else { -// prep_segment->n_step -= ceil(pl_prep_block->acceleration*(0.5*TIME_PER_SEGMENT*TIME_PER_SEGMENT/(60*60))/mm_per_step); -// } - - prep_segment->n_step = 15; //floor( (exit_speed*approx_time)/mm_per_step ); -// prep_segment->n_step = max(prep_segment->n_step,MINIMUM_STEPS_PER_BLOCK); // Ensure it moves for very slow motions? -// prep_segment->n_step = min(prep_segment->n_step,MAXIMUM_STEPS_PER_BLOCK); // Prevent unsigned int8 overflow. + // Compute the number of steps in the prepped segment based on the approximate current rate. The execution + // time of each segment should be about every ACCELERATION_TICK. + // NOTE: The d_next divide cancels out the INV_TIME_MULTIPLIER and converts the rate value to steps. + // NOTE: As long as the ACCELERATION_TICKS_PER_SECOND is valid, n_step should never exceed 255. + prep_segment->n_step = ceil(max(MINIMUM_STEP_RATE,st_prep_data->current_approx_rate)* + (ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND)/st_prep_data->d_next); + prep_segment->n_step = max(prep_segment->n_step,MINIMUM_STEPS_PER_SEGMENT); // Ensure it moves for very slow motions? + // prep_segment->n_step = min(prep_segment->n_step,MAXIMUM_STEPS_PER_BLOCK); // Prevent unsigned int8 overflow. // Check if n_step exceeds steps remaining in planner block. If so, truncate. if (prep_segment->n_step > st_prep_data->step_events_remaining) { prep_segment->n_step = st_prep_data->step_events_remaining; - - // Don't need to compute last velocity, since it will be refreshed with a new block. } - // Check if n_step exceeds decelerate point in block. Need to perform this so that the - // ramp counters are reset correctly in the stepper algorithm. Can be 1 step, but should - // be OK since it is likely moving at a fast rate already. + // Check if n_step crosses decelerate point in block. If so, truncate to ensure the deceleration + // ramp counters are set correctly during execution. if (st_prep_data->decelerate_after > 0) { if (prep_segment->n_step > st_prep_data->decelerate_after) { prep_segment->n_step = st_prep_data->decelerate_after; } -// !!! Doesn't work. Remove if not using. -// if (last_velocity < last_nominal_v) { -// // !!! Doesn't work since distance changes and gets truncated. -// last_velocity += pl_prep_block->acceleration*(TIME_PER_SEGMENT/(60*60)); // In acceleration ramp. -// if {last_velocity > last_nominal_v) { last_velocity = last_nominal_v; } // Set to cruising. -// } -// } else { // In deceleration ramp -// last_velocity -= pl_prep_block->acceleration*(TIME_PER_SEGMENT/(60*60)); - } else { - if (st_prep_data->decelerate_after == 0) { prep_segment->flag |= ST_DECEL; } } - st_prep_data->decelerate_after -= prep_segment->n_step; // Update stepper block variables. + st_prep_data->decelerate_after -= prep_segment->n_step; st_prep_data->step_events_remaining -= prep_segment->n_step; + + // Check for end of planner block if ( st_prep_data->step_events_remaining == 0 ) { - prep_segment->flag |= ST_END_OF_BLOCK; + // Set EOB bitflag so stepper algorithm discards the planner block after this segment completes. + prep_segment->flag |= SEGMENT_END_OF_BLOCK; // Move planner pointer to next block and flag to load a new block for the next segment. pl_prep_index = next_block_pl_index(pl_prep_index); pl_prep_block = NULL; @@ -719,6 +719,10 @@ void st_prep_buffer() // New step segment completed. Increment segment buffer indices. segment_buffer_head = segment_next_head; segment_next_head = next_block_index(segment_buffer_head); + +// long a = prep_segment->n_step; +// printInteger(a); +// printString(" "); } } diff --git a/stepper_new2.c b/stepper_new2.c deleted file mode 100644 index c248d4d..0000000 --- a/stepper_new2.c +++ /dev/null @@ -1,601 +0,0 @@ -/* - stepper.c - stepper motor driver: executes motion plans using stepper motors - Part of Grbl - - Copyright (c) 2011-2013 Sungeun K. Jeon - Copyright (c) 2009-2011 Simen Svale Skogsrud - - 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 - (at your option) any later version. - - Grbl is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with Grbl. If not, see . -*/ - -#include -#include "stepper.h" -#include "config.h" -#include "settings.h" -#include "planner.h" -#include "nuts_bolts.h" - -// Some useful constants -#define TICKS_PER_MICROSECOND (F_CPU/1000000) -#define CRUISE_RAMP 0 -#define ACCEL_RAMP 1 -#define DECEL_RAMP 2 - -#define LOAD_NOOP 0 -#define LOAD_LINE 1 -#define LOAD_BLOCK 2 - -// Stepper state variable. Contains running data and trapezoid variables. -typedef struct { - // Used by the bresenham line algorithm - int32_t counter_x, // Counter variables for the bresenham line tracer - counter_y, - counter_z; - int32_t step_events_remaining; // Steps remaining in line motion - - // Used by inverse time algorithm - int32_t delta_d; // Inverse time distance traveled per interrupt tick - int32_t d_counter; // Inverse time distance traveled since last step event - int32_t d_per_tick; - - // Used by the stepper driver interrupt - uint8_t execute_step; // Flags step execution for each interrupt. - uint8_t step_pulse_time; // Step pulse reset time after step rise - uint8_t out_bits; // The next stepping-bits to be output - uint8_t load_flag; -} stepper_t; -static stepper_t st; - -#define STEPPER_BUFFER_SIZE 5 -typedef struct { - int32_t event_count; - int32_t rate; - uint8_t end_of_block; - uint8_t tick_count; - - int32_t initial_rate; // The step rate at start of block - int32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive) - int32_t decelerate_after; // The index of the step event on which to start decelerating - int32_t nominal_rate; // The nominal step rate for this block in step_events/minute - int32_t d_next; // Scaled distance to next step - -} stepper_buffer_t; -static stepper_buffer_t step_buffer[STEPPER_BUFFER_SIZE]; - -static volatile uint8_t step_buffer_tail; -static uint8_t step_buffer_head; -static uint8_t step_next_head; - -// NOTE: If the main interrupt is guaranteed to be complete before the next interrupt, then -// this blocking variable is no longer needed. Only here for safety reasons. -static volatile uint8_t busy; // True when "Stepper Driver Interrupt" is being serviced. Used to avoid retriggering that handler. -static plan_block_t *plan_current_block; // A pointer to the planner block currently being traced - - -/* __________________________ - /| |\ _________________ ^ - / | | \ /| |\ | - / | | \ / | | \ s - / | | | | | \ p - / | | | | | \ e - +-----+------------------------+---+--+---------------+----+ e - | BLOCK 1 | BLOCK 2 | d - - time -----> - - The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta - until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after - after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as - +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. -*/ - -// Stepper state initialization. Cycle should only start if the st.cycle_start flag is -// enabled. Startup init and limits call this function but shouldn't start the cycle. -void st_wake_up() -{ - // Enable steppers by resetting the stepper disable port - if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { - STEPPERS_DISABLE_PORT |= (1<> 3); - // Enable stepper driver interrupt - st.execute_step = false; - TCNT2 = 0; // Clear Timer2 - TIMSK2 |= (1<direction_bits ^ settings.invert_mask; - st.execute_step = true; // Set flag to set direction bits. - - st.counter_x = (plan_current_block->step_event_count >> 1); - st.counter_y = st.counter_x; - st.counter_z = st.counter_x; - - // This is correct. Sets the total time before the next step occurs. - st.counter_d = plan_current_block->d_next; // d_next always greater than delta_d. - - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid - - } - - st.load_flag = LOAD_NOOP; // Line motion loaded. Set no-operation flag until complete. - - } else { - // Can't discard planner block here if a feed hold stops in middle of block. - st_go_idle(); - bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end - return; // Nothing to do but exit. - } - - } - - // Iterate inverse time counter. Triggers each Bresenham step event. - st.counter_d -= st.delta_d; - - // Execute Bresenham step event, when it's time to do so. - if (st.counter_d < 0) { - st.counter_d += plan_current_block->d_next; // Reload inverse time counter - - st.out_bits = plan_current_block->direction_bits; // Reset out_bits and reload direction bits - st.execute_step = true; - - // Execute step displacement profile by Bresenham line algorithm - st.counter_x -= plan_current_block->steps[X_AXIS]; - if (st.counter_x < 0) { - st.out_bits |= (1<step_event_count; - if (st.out_bits & (1<steps[Y_AXIS]; - if (st.counter_y < 0) { - st.out_bits |= (1<step_event_count; - if (st.out_bits & (1<steps[Z_AXIS]; - if (st.counter_z < 0) { - st.out_bits |= (1<step_event_count; - if (st.out_bits & (1<step_event_count; - //st.step_events_remaining = st.event_count; - - // Convert new block to stepper variables. - // NOTE: This data can change mid-block from normal planner updates and feedrate overrides. Must - // be maintained as these execute. - // TODO: The initial rate needs to be sent back to the planner to update the entry speed - block->initial_rate = ceil(sqrt(plan_current_block->entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) - block->nominal_rate = ceil(plan_current_block->nominal_speed*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) - - // This data doesn't change. Could be performed in the planner, but fits nicely here. - // Although, acceleration can change for S-curves. So keep it here. - block->rate_delta = ceil(plan_current_block->acceleration* - ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) - // This definitely doesn't change, but could be precalculated in a way to help some of the - // math in this handler, i.e. millimeters per step event data. - block->d_next = ceil((plan_current_block->millimeters*INV_TIME_MULTIPLIER)/plan_current_block->step_event_count); // (mult*mm/step) - - // During feed hold, do not update Ranade counter, rate, or ramp type. Keep decelerating. - if (sys.state == STATE_CYCLE) { } - } - - -// Track instead the trapezoid line and use the average of the entry and exit velocities -// to determine step rate. This should take care of the deceleration issue automatically... -// i think. -// First need to figure out what type of profile is segment is, i.e. acceleration only, accel -// to decel triangle, cruise to decel, or all three. May need more profile data to compute this -// from the planner itself, like accelerate until. -// Another issue. This is only tracking the velocity profile, not the distance covered over that -// time period. This can lead to an unsynchronized velocity profile and steps executed. But, -// how much drift is there really? Enough to be a problem? Not sure. I would think typical -// drift would be on the order of a few steps, more depending on step resolution. -entry_rate = last_exit_rate; -time = 250 ISR ticks per acceleration tick. -distance = 0; -if (distance_traveled < accelerate_until) - exit_rate = entry_rate + acceleration*time; - if (exit_rate > nominal_rate) { - exit_rate = nominal_rate; - time = 2*(accelerate_until-distance_travel)/(entry_rate+nominal_rate); - // distance = accelerate_until; // Enforce distance? - // Truncate this segment. - } -} else if (distance_traveled >= decelerate_after) { - if (accelerate_until == decelerate_after) { - time = last time; - exit_rate = entry_rate; - } else { - exit_rate = entry_rate - acceleration*time; - } -} else { - exit_rate = nominal_rate; // Just cruise - distance = nominal_rate*time; - if (distance > decelerate_after) { // Truncate segment at nominal rate. - time = (decelerate_after-distance_traveled)/(nominal_rate); - distance = decelerate_after; - } -} -mean_rate = 0.5*(entry_rate+exit_rate); -distance = mean_rate*time; - - -if (entry_rate < nominal_rate) { - if (entry_distance < decelerate_after) { // Acceleration case - exit_rate = entry_rate + acceleration*time - exit_rate = min(exit_rate,nominal_rate); -mean_rate = 0.5*(entry_rate + exit_rate); -distance = mean_rate*time; -if (distance > decelerate_after) { - exit_rate = - - - // If the MINIMUM_STEP_RATE is less than ACCELERATION_TICKS_PER_SECOND then there can be - // rate adjustements that have less than one step per tick. - // How do you deal with the remainer? -time = 250 ISR ticks per acceleration tick. (30000/120) -delta_d*time // mm per acceleration tick -delta_d*time/d_next // number of steps/acceleration_tick. Chance of integer overflow. -delta_d*time/d_next + last_remainder. // steps/acceleration_tick. -n_step*d_next/delta_d // number of ISR ticks for enforced n_steps. - -// In floating point? Then convert? -// Requires exact millimeters. Roundoff might be a problem. But could be corrected by just -// checking if the total step event counts are performed. -// Could be limited by float conversion and about 1e7 steps per block. -line_mm = feed_rate / acc_tick // mm per acc_tick -n_steps = floor(line_mm * step_event_remaining/millimeters_remaining) // steps. float 7.2 digits|int32 10 digits -millimeters_remaining -= line_mm; -step_events_remaining -= n_steps; - -// There doesn't seem to be a way to avoid this divide here. -line_mm = feed_rate / acc_tick // mm per acc_tick -n_steps = floor( (line_mm+line_remainder) * step_event_count/millimeters) // steps. float 7.2 digits|int32 10 digits -line_remainder = line_mm - n_steps*(millimeters/step_event_count); - -// Need to handle when rate is very very low, i.e. less than one step per accel tick. -// Could be bounded by MINIMUM_STEP_RATE. - -// 1. Figure out how many steps occur exactly within n ISR ticks. -// 2. Account for step-time remainder for next line motion exactly. -// 3. At the end of block, determine exact number of ISR ticks to finish the steps. Or,\ - have the ISR track steps to exit on time. It would require an extra counter. - -// NOTE: There doesn't seem to be a great way to figure out how many steps occur within -// a set number of ISR ticks. Numerical round-off and CPU overhead always seems to be a -// critical problem. So, either numerical round-off checks could be made to account for -// them, while CPU overhead could be minimized in some way, or we can flip the algorithm -// around to have the stepper algorithm track number of steps over an indeterminant amount -// of time instead. -// In other words, we use the planner velocity floating point data to get an estimate of -// the number of steps we want to execute. We then back out the approximate velocity for -// the planner to use, which should be much more robust to round-off error. The main problem -// now is that we are loading the stepper algorithm to handle acceleration now, rather than -// pre-calculating with the main program. This approach does make sense in the way that -// planner velocities and stepper profiles can be traced more accurately. -// Which is better? Very hard to tell. The time-based algorithm would be able to handle -// Bresenham step adaptive-resolution much easier and cleaner. Whereas, the step-based would -// require some additional math in the stepper algorithm to adjust on the fly, plus adaptation -// would occur in a non-deterministic manner. -// I suppose it wouldn't hurt to build both to see what's better. Just a lot more work. - -feed_rate/120 = millimeters per acceleration tick - - - steps? - d_next // (mult*mm/step) - rate // (mult*mm/isr_tic) - rate/d_next // step/isr_tic - - if (plan_current_block->step_events_remaining <= plan_current_block->decelerate_after) { - // Determine line segment velocity and associated inverse time counter. - if (step_block.ramp_type == ACCEL_RAMP) { // Adjust velocity for acceleration - step_block.delta_d += plan_current_block->rate_delta; - if (step_block.delta_d >= plan_current_block->nominal_rate) { // Reached cruise state. - step_block.ramp_type = CRUISE_RAMP; - step_block.delta_d = plan_current_block->nominal_rate; // Set cruise velocity - } - } - } else { // Adjust velocity for deceleration - if (step_block.delta_d > plan_current_block->rate_delta) { - step_block.delta_d -= plan_current_block->rate_delta; - } else { - step_block.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow. - } - } - - // Incorrect. Can't overwrite delta_d. Needs to override instead. - if (step_block.delta_d < MINIMUM_STEP_RATE) { step_block.delta_d = MINIMUM_STEP_RATE; } - - /* - Compute the number of steps needed to complete this move over the move time, i.e. - ISR_TICKS_PER_ACCELERATION_TICK. - - The first block in the buffer is half of the move time due to midpoint rule. - - Check if this reaches the deceleration after location. If so, truncate move. Also, - if this is a triangle move, double the truncated move to stay with midpoint rule. - NOTE: This can create a stepper buffer move down to just one step in length. - - Update the planner block entry speed for the planner to compute from end of the - stepper buffer location. - - If a feed hold occurs, begin to enforce deceleration, while enforcing the above rules. - When the deceleration is complete, all we need to do is update the planner block - entry speed and force a replan. - */ - - // Planner block move completed. - // TODO: planner buffer tail no longer needs to be volatile. only accessed by main program. - if (st.step_events_remaining == 0) { - plan_current_block = NULL; // Set flag that we are done with this planner block. - plan_discard_current_block(); - } - - - - - - step_buffer_head = step_next_head; - step_next_head = next_block_index(step_buffer_head); - - } -} diff --git a/stepper_new_dual_ISR.c b/stepper_new_dual_ISR.c deleted file mode 100644 index 87f87bc..0000000 --- a/stepper_new_dual_ISR.c +++ /dev/null @@ -1,425 +0,0 @@ -/* - stepper.c - stepper motor driver: executes motion plans using stepper motors - Part of Grbl - - Copyright (c) 2011-2013 Sungeun K. Jeon - Copyright (c) 2009-2011 Simen Svale Skogsrud - - 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 - (at your option) any later version. - - Grbl is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with Grbl. If not, see . -*/ - -#include -#include "stepper.h" -#include "config.h" -#include "settings.h" -#include "planner.h" - -// Some useful constants -#define TICKS_PER_MICROSECOND (F_CPU/1000000) -#define CRUISE_RAMP 0 -#define ACCEL_RAMP 1 -#define DECEL_RAMP 2 - -// Stepper state variable. Contains running data and trapezoid variables. -typedef struct { - // Used by the bresenham line algorithm - int32_t counter[N_AXIS]; // Counter variables for the bresenham line tracer - uint32_t event_count; // Total event count. Retained for feed holds. - uint32_t step_events_remaining; // Steps remaining in motion - - // Used by Pramod Ranade inverse time algorithm - int32_t delta_d; // Ranade distance traveled per interrupt tick - int32_t d_counter; // Ranade distance traveled since last step event - uint8_t ramp_count; // Acceleration interrupt tick counter. - uint8_t ramp_type; // Ramp type variable. - uint8_t execute_step; // Flags step execution for each interrupt. - -} stepper_t; -static stepper_t st; -static block_t *current_block; // A pointer to the block currently being traced - -// Used by the stepper driver interrupt -static uint8_t step_pulse_time; // Step pulse reset time after step rise -static uint8_t out_bits; // The next stepping-bits to be output - -// NOTE: If the main interrupt is guaranteed to be complete before the next interrupt, then -// this blocking variable is no longer needed. Only here for safety reasons. -static volatile uint8_t busy; // True when "Stepper Driver Interrupt" is being serviced. Used to avoid retriggering that handler. - -// __________________________ -// /| |\ _________________ ^ -// / | | \ /| |\ | -// / | | \ / | | \ s -// / | | | | | \ p -// / | | | | | \ e -// +-----+------------------------+---+--+---------------+----+ e -// | BLOCK 1 | BLOCK 2 | d -// -// time -----> -// -// The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta -// until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after -// after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as -// +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. - - -// Stepper state initialization. Cycle should only start if the st.cycle_start flag is -// enabled. Startup init and limits call this function but shouldn't start the cycle. -void st_wake_up() -{ - // Enable steppers by resetting the stepper disable port - if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { - STEPPERS_DISABLE_PORT |= (1<> 3); - // Enable stepper driver interrupt - st.execute_step = false; - TCNT0 = 0; // Clear Timer2 - TIMSK0 |= (1<d_next; - st.execute_step = true; - - // Configure next step - out_bits = current_block->direction_bits; // Reset out_bits and reload direction bits - - // Execute step displacement profile by Bresenham line algorithm - st.counter[X_AXIS] -= current_block->steps_x; // Doesn't change when set up. - if (st.counter[X_AXIS] < 0) { - out_bits |= (1<steps_y; - if (st.counter[Y_AXIS] < 0) { - out_bits |= (1<steps_z; - if (st.counter[Z_AXIS] < 0) { - out_bits |= (1<direction_bits ^ settings.invert_mask; - st.execute_step = true; // Set flag to set direction bits. - - // Initialize Bresenham variables - st.counter_x = (current_block->step_event_count >> 1); - st.counter_y = st.counter_x; - st.counter_z = st.counter_x; - st.event_count = current_block->step_event_count; - st.step_events_remaining = st.event_count; - - // During feed hold, do not update inverse time counter, rate, or ramp type. Keep decelerating. - if (sys.state == STATE_CYCLE) { - // Initialize Ranade variables - st.d_counter = current_block->d_next; - st.delta_d = current_block->initial_rate; - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; - - // Initialize ramp type. - if (st.step_events_remaining == current_block->decelerate_after) { st.ramp_type = DECEL_RAMP; } - else if (st.delta_d == current_block->nominal_rate) { st.ramp_type = CRUISE_RAMP; } - else { st.ramp_type = ACCEL_RAMP; } - } - - } else { - st_go_idle(); - bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end - return; // Nothing to do but exit. - } - } - - // Adjust inverse time counter for ac/de-celerations - if (st.ramp_type) { - // Tick acceleration ramp counter - st.ramp_count--; - if (st.ramp_count == 0) { - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter - if (st.ramp_type == ACCEL_RAMP) { // Adjust velocity for acceleration - st.delta_d += current_block->rate_delta; - if (st.delta_d >= current_block->nominal_rate) { // Reached cruise state. - st.ramp_type = CRUISE_RAMP; - st.delta_d = current_block->nominal_rate; // Set cruise velocity - } - } else if (st.ramp_type == DECEL_RAMP) { // Adjust velocity for deceleration - if (st.delta_d > current_block->rate_delta) { - st.delta_d -= current_block->rate_delta; - } else { - st.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow. - } - } - } - } - - - // Check for feed hold state and execute accordingly. - if (sys.state == STATE_HOLD) { - if (st.ramp_type != DECEL_RAMP) { - st.ramp_type = DECEL_RAMP; - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; - } - if (st.delta_d <= current_block->rate_delta) { - st_go_idle(); - bit_true(sys.execute,EXEC_CYCLE_STOP); - return; - } - } - - if (st.ramp_type != DECEL_RAMP) { - // Acceleration and cruise handled by ramping. Just check for deceleration. - if (st.step_events_remaining <= current_block->decelerate_after) { - st.ramp_type = DECEL_RAMP; - if (st.step_events_remaining == current_block->decelerate_after) { - if (st.delta_d == current_block->nominal_rate) { - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid - } else { - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; // Set ramp counter for triangle - } - } - } - } - } else { - - busy = false; -} - - -// The Stepper Port Reset Interrupt: Timer2 OVF interrupt handles the falling edge of the -// step pulse. This should always trigger before the next Timer0 COMPA interrupt and independently -// finish, if Timer0 is disabled after completing a move. -ISR(TIMER2_OVF_vect) -{ - STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | (settings.invert_mask & STEP_MASK); - TCCR2B = 0; // Disable timer until needed. -} - - -// Reset and clear stepper subsystem variables -void st_reset() -{ - memset(&st, 0, sizeof(st)); - current_block = NULL; - busy = false; -} - - -// Initialize and start the stepper motor subsystem -void st_init() -{ - // Configure directions of interface pins - STEPPING_DDR |= STEPPING_MASK; - STEPPING_PORT = (STEPPING_PORT & ~STEPPING_MASK) | settings.invert_mask; - STEPPERS_DISABLE_DDR |= 1<. -*/ - -#include -#include "stepper.h" -#include "config.h" -#include "settings.h" -#include "planner.h" -#include "nuts_bolts.h" - -// Some useful constants -#define TICKS_PER_MICROSECOND (F_CPU/1000000) - -#define RAMP_NOOP_CRUISE 0 -#define RAMP_ACCEL 1 -#define RAMP_DECEL 2 - -#define LOAD_NOOP 0 -#define LOAD_LINE 1 -#define LOAD_BLOCK 2 - -#define ST_NOOP 0 -#define ST_END_OF_BLOCK 1 - -// Stepper state variable. Contains running data and trapezoid variables. -typedef struct { - // Used by the bresenham line algorithm - int32_t counter_x, // Counter variables for the bresenham line tracer - counter_y, - counter_z; - int8_t segment_steps_remaining; // Steps remaining in line motion - - // Used by inverse time algorithm to track step rate - int32_t counter_d; // Inverse time distance traveled since last step event - int32_t delta_d; // Inverse time distance traveled per interrupt tick - int32_t d_per_tick; - - // Used by the stepper driver interrupt - uint8_t execute_step; // Flags step execution for each interrupt. - uint8_t step_pulse_time; // Step pulse reset time after step rise - uint8_t out_bits; // The next stepping-bits to be output - uint8_t load_flag; - - uint8_t ramp_count; - uint8_t ramp_type; -} stepper_t; -static stepper_t st; - -#define SEGMENT_BUFFER_SIZE 5 -// Stores stepper buffer common data. Can change planner mid-block in special conditions. -typedef struct { - int32_t step_events_remaining; // Tracks step event count for the executing planner block - int32_t d_next; // Scaled distance to next step - float mm_per_step; -} st_data_t; -static st_data_t segment_data[SEGMENT_BUFFER_SIZE]; - -// Primary stepper motion buffer -typedef struct { - uint8_t n_step; - int32_t rate; - uint8_t st_data_index; - uint8_t flag; -} st_segment_t; -static st_segment_t segment_buffer[SEGMENT_BUFFER_SIZE]; - -static volatile uint8_t segment_buffer_tail; -static uint8_t segment_buffer_head; -static uint8_t segment_next_head; - -static volatile uint8_t busy; // Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though. -static plan_block_t *pl_current_block; // A pointer to the planner block currently being traced -static st_segment_t *st_current_segment; -static st_data_t *st_current_data; - -static plan_block_t *pl_prep_block; // A pointer to the planner block being prepped into the stepper buffer -static uint8_t pl_prep_index; -static st_data_t *st_prep_data; -static uint8_t st_data_prep_index; - - -// Returns the index of the next block in the ring buffer -// NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication. -static uint8_t next_block_index(uint8_t block_index) -{ - block_index++; - if (block_index == SEGMENT_BUFFER_SIZE) { block_index = 0; } - return(block_index); -} - - -/* __________________________ - /| |\ _________________ ^ - / | | \ /| |\ | - / | | \ / | | \ s - / | | | | | \ p - / | | | | | \ e - +-----+------------------------+---+--+---------------+----+ e - | BLOCK 1 | BLOCK 2 | d - - time -----> - - The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta - until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after - after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as - +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. -*/ - -// Stepper state initialization. Cycle should only start if the st.cycle_start flag is -// enabled. Startup init and limits call this function but shouldn't start the cycle. -void st_wake_up() -{ - // Enable steppers by resetting the stepper disable port - if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { - STEPPERS_DISABLE_PORT |= (1<> 3); - // Enable stepper driver interrupt - st.execute_step = false; - TCNT2 = 0; // Clear Timer2 - TIMSK2 |= (1<n_step; - st.delta_d = st_current_segment->rate; - - // Check if the counters need to be reset for a new planner block - if (st.load_flag == LOAD_BLOCK) { - pl_current_block = plan_get_current_block(); // Should always be there. Stepper buffer handles this. - st_current_data = &segment_data[st_current_segment->st_data_index]; - - // Initialize direction bits for block - st.out_bits = pl_current_block->direction_bits ^ settings.invert_mask; - st.execute_step = true; // Set flag to set direction bits upon next ISR tick. - - // Initialize Bresenham line counters - st.counter_x = (pl_current_block->step_event_count >> 1); - st.counter_y = st.counter_x; - st.counter_z = st.counter_x; - - // Initialize inverse time and step rate counter data - st.counter_d = st_current_data->d_next; // d_next always greater than delta_d. - } - st.load_flag = LOAD_NOOP; // Motion loaded. Set no-operation flag until complete. - - } else { - // Can't discard planner block here if a feed hold stops in middle of block. - st_go_idle(); - bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end - return; // Nothing to do but exit. - } - - } - - // Iterate inverse time counter. Triggers each Bresenham step event. - st.counter_d -= st.delta_d; - - // Execute Bresenham step event, when it's time to do so. - if (st.counter_d < 0) { - st.counter_d += st_current_data->d_next; // Reload inverse time counter - - st.out_bits = pl_current_block->direction_bits; // Reset out_bits and reload direction bits - st.execute_step = true; - - // Execute step displacement profile by Bresenham line algorithm - st.counter_x -= pl_current_block->steps[X_AXIS]; - if (st.counter_x < 0) { - st.out_bits |= (1<step_event_count; - if (st.out_bits & (1<steps[Y_AXIS]; - if (st.counter_y < 0) { - st.out_bits |= (1<step_event_count; - if (st.out_bits & (1<steps[Z_AXIS]; - if (st.counter_z < 0) { - st.out_bits |= (1<step_event_count; - if (st.out_bits & (1<flag == ST_END_OF_BLOCK) { - plan_discard_current_block(); - st.load_flag = LOAD_BLOCK; - } else { - st.load_flag = LOAD_LINE; - } - - // Discard current block - if (segment_buffer_head != segment_buffer_tail) { - segment_buffer_tail = next_block_index( segment_buffer_tail ); - } - - // NOTE: sys.position updates could be done here. The bresenham counters can have - // their own fast 8-bit addition-only counters. Here we would check the direction and - // apply it to sys.position accordingly. However, this could take too much time. - - } - - st.out_bits ^= settings.invert_mask; // Apply step port invert mask - } - busy = false; -// SPINDLE_ENABLE_PORT ^= 1<step_events_remaining); - st.ramp_type = RAMP_ACCEL; - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; - st.delta_d = 0; - sys.state = STATE_QUEUED; - } else { - sys.state = STATE_IDLE; - } -} - - -/* Preps stepper buffer. Called from main program. - - NOTE: There doesn't seem to be a great way to figure out how many steps occur within - a set number of ISR ticks. Numerical round-off and CPU overhead always seems to be a - critical problem. So, either numerical round-off checks could be made to account for - them, while CPU overhead could be minimized in some way, or we can flip the algorithm - around to have the stepper algorithm track number of steps over an indeterminant amount - of time instead. - In other words, we use the planner velocity floating point data to get an estimate of - the number of steps we want to execute. We then back out the approximate velocity for - the planner to use, which should be much more robust to round-off error. The main problem - now is that we are loading the stepper algorithm to handle acceleration now, rather than - pre-calculating with the main program. This approach does make sense in the way that - planner velocities and stepper profiles can be traced more accurately. - Which is better? Very hard to tell. The time-based algorithm would be able to handle - Bresenham step adaptive-resolution much easier and cleaner. Whereas, the step-based would - require some additional math in the stepper algorithm to adjust on the fly, plus adaptation - would occur in a non-deterministic manner. - I suppose it wouldn't hurt to build both to see what's better. Just a lot more work. - - TODO: Need to describe the importance of continuations of step pulses between ramp states - and planner blocks. This has to do with Alden's problem with step "phase". The things I've - been doing here limit this phase issue by truncating some of the ramp timing for certain - events like deceleration initialization and end of block. -*/ -void st_prep_buffer() -{ - while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer. - - // Determine if we need to load a new planner block. - if (pl_prep_block == NULL) { - pl_prep_block = plan_get_block_by_index(pl_prep_index); - if (pl_prep_block == NULL) { return; } // No more planner blocks. Let stepper finish out. - - // Prepare commonly shared planner block data for the ensuing step buffer moves - st_data_prep_index = next_block_index(st_data_prep_index); - st_prep_data = &segment_data[st_data_prep_index]; - - // Initialize Bresenham variables - st_prep_data->step_events_remaining = pl_prep_block->step_event_count; - - // Convert new block to stepper variables. - // NOTE: This data can change mid-block from normal planner updates and feedrate overrides. Must - // be maintained as these execute. - // TODO: If the planner updates this block, particularly from a deceleration to an acceleration, - // we must reload the initial rate data, such that the velocity profile is re-constructed correctly. -// st_prep_data->initial_rate = ceil(sqrt(pl_prep_block->entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) -// st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) - - // This data doesn't change. Could be performed in the planner, but fits nicely here. - // Although, acceleration can change for S-curves. So keep it here. -// st_prep_data->rate_delta = ceil(pl_prep_block->acceleration* -// ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) - // This definitely doesn't change, but could be precalculated in a way to help some of the - // math in this handler, i.e. millimeters per step event data. - st_prep_data->d_next = ceil((pl_prep_block->millimeters*INV_TIME_MULTIPLIER)/pl_prep_block->step_event_count); // (mult*mm/step) - - st_prep_data->mm_per_step = pl_prep_block->millimeters/pl_prep_block->step_event_count; - } - - - /* - // Check if planner has changed block exit parameters. If so, then we have to update the - // velocity profile for the remainder of this block. Otherwise, the block hasn't changed. - if (exit_speed_sqr != last_exit_speed_sqr) { - intersect_distance = 0.5*( millimeters + (entry_speed_sqr-exit_speed_sqr)/(2*acceleration) ); - if (intersect_distance <= 0) { // Deceleration only. - final_rate_sqr = initial_rate_sqr - 2*acceleration*segment_distance; - block->decelerate_after = 0; - } else { - decelerate_after = (block->nominal_speed_sqr - exit_speed_sqr)/(2*block->acceleration); - if (decelerate_after > intersect_distance) { decelerate_after = intersect_distance; } - if (decelerate_after > block->millimeters) { decelerate_after = block->millimeters; } - } - } - - - - - - n_step = 100; // Estimate distance we're going to travel in this segment - if (n_step > step_events_remaining) { n_step = step_events_remaining; }; - - segment_distance = n_step*mm_per_step; // True distance traveled - - // ISSUE: Either weighted average speed or truncated segments must be used. Otherwise - // accelerations, in particular high value, will not perform correctly. - if (distance_traveled < decelerate_after) { - if (segment_distance + distance_traveled > decelerate_after) { - n_step = ceil((decelerate_after-distance_traveled)/mm_per_step); - segment_distance = n_step*mm_per_step; - } - } - - - - - // based on time? - time = incremental time; - v_exit = v_entry + acceleration*time; - if (v_exit > v_nominal) { - time_accel = (v_nominal-v_entry)/acceleration; - distance = v_entry*time_accel + 0.5*acceleration*time_accel**2; - time_remaining = time - time_accel; - } - distance = v_entry*time + 0.5*acceleration*time*time; - - - t_accel = (v_nominal - v_entry) / acceleration; - t_decel = (v_exit - v_nominal) / -acceleration; - dt = (v_entry-v_exit)/acceleration; - if - - - - - // What's the speed of this segment? Computing per segment can get expensive, but this - // would allow S-curves to be installed pretty easily here. - if (initial_speed < nominal_speed) { - if (initial_distance < decelerate_after) { // Acceleration - exit_speed = sqrt(initial_speed*initial_speed + 2*acceleration*distance); - if (exit_speed > nominal_speed) { exit_speed = nominal_speed; } - } else { // Deceleration - exit_speed = sqrt(initial_speed*initial_speed - 2*acceleration*distance); - } - average_speed = 0.5*(initial_speed + exit_speed); - } else { // Cruise - average_speed = nominal_speed; - } - segment_rate = ceil(average_speed*(INV_TIME_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) - if (segment_rate < MINIMUM_STEP_RATE) { segment_rate = MINIMUM_STEP_RATE; } - - - - - */ - - - - /* - TODO: Need to check for a planner flag to indicate a change to this planner block. - If so, need to check for a change in acceleration state, from deceleration to acceleration, - to reset the stepper ramp counters and the initial_rate data to trace the new - ac/de-celeration profile correctly. - No change conditions: - - From nominal speed to acceleration from feedrate override - - From nominal speed to new deceleration. - - From acceleration to new deceleration point later or cruising point. - - From acceleration to immediate deceleration? Can happen during feedrate override - and slowing down, but likely ok by enforcing the normal ramp counter protocol. - Change conditions: - - From deceleration to acceleration, i.e. common with jogging when new blocks are added. - */ - - st_segment_t *st_prep_block = &segment_buffer[segment_buffer_head]; - st_prep_block->st_data_index = st_data_prep_index; - - // TODO: How do you cheaply compute n_step without a sqrt()? Could be performed as 'bins'. - st_prep_block->n_step = 100; //floor( (exit_speed*approx_time)/mm_per_step ); -// st_segment->n_step = max(st_segment->n_step,MINIMUM_STEPS_PER_BLOCK); // Ensure it moves for very slow motions? -// st_segment->n_step = min(st_segment->n_step,MAXIMUM_STEPS_PER_BLOCK); // Prevent unsigned int8 overflow. - - // Check if n_step exceeds steps remaining in planner block. If so, truncate. - if (st_prep_block->n_step > st_prep_data->step_events_remaining) { - st_prep_block->n_step = st_prep_data->step_events_remaining; - } - - // Check if n_step exceeds decelerate point in block. Need to perform this so that the - // ramp counters are reset correctly in the stepper algorithm. Can be 1 step, but should - // be OK since it is likely moving at a fast rate already. - if (st_prep_block->n_step > pl_prep_block->decelerate_after) { - st_prep_block->n_step = pl_prep_block->decelerate_after; - } - - float distance, exit_speed_sqr; - distance = st_prep_block->n_step*st_prep_data->mm_per_step; // Always greater than zero - if (st_prep_data->step_events_remaining >= pl_prep_block->decelerate_after) { - exit_speed_sqr = pl_prep_block->entry_speed_sqr - 2*pl_prep_block->acceleration*distance; - // Set ISR tick reset flag for deceleration ramp. - } else { // Acceleration or cruising ramp - if (pl_prep_block->entry_speed_sqr < pl_prep_block->nominal_speed_sqr) { - exit_speed_sqr = pl_prep_block->entry_speed_sqr + 2*pl_prep_block->acceleration*distance; - if (exit_speed_sqr > pl_prep_block->nominal_speed_sqr) { exit_speed_sqr = pl_prep_block->nominal_speed_sqr; } - } else { - exit_speed_sqr = pl_prep_block->nominal_speed_sqr; - } - } - - - // Adjust inverse time counter for ac/de-celerations - if (st.ramp_type) { - st.ramp_count--; // Tick acceleration ramp counter - if (st.ramp_count == 0) { // Adjust step rate when its time - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter - if (st.ramp_type == RAMP_ACCEL) { // Adjust velocity for acceleration - st.delta_d += st_current_data->rate_delta; - if (st.delta_d >= st_current_data->nominal_rate) { // Reached nominal rate. - st.delta_d = st_current_data->nominal_rate; // Set cruising velocity - st.ramp_type = RAMP_NOOP_CRUISE; // Set ramp flag to ignore - } - } else { // Adjust velocity for deceleration - if (st.delta_d > st_current_data->rate_delta) { - st.delta_d -= st_current_data->rate_delta; - } else { - - // Moving near zero feed rate. Gracefully slow down. - st.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow. - - // Check for and handle feed hold exit? At this point, machine is stopped. - - } - } - // Finalize adjusted step rate. Ensure minimum. - if (st.delta_d < MINIMUM_STEP_RATE) { st.d_per_tick = MINIMUM_STEP_RATE; } - else { st.d_per_tick = st.delta_d; } - } - } - // During feed hold, do not update rate or ramp type. Keep decelerating. - if (sys.state == STATE_CYCLE) { - st.delta_d = st_current_data->initial_rate; - if (st.delta_d == st_current_data->nominal_rate) { - ramp_type = RAMP_NOOP_CRUISE; - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid - } - // Acceleration and cruise handled by ramping. Just check for deceleration. - if (st_current_segment->flag == ST_NOOP) { - if (st.ramp_type == RAMP_NOOP_CRUISE) { - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid - } else { - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; // Set ramp counter for triangle - } - st.ramp_type = RAMP_DECEL; - } - - - - - - // Update planner block variables. - pl_prep_block->entry_speed_sqr = max(0.0,exit_speed_sqr); -// pl_prep_block->max_entry_speed_sqr = exit_speed_sqr; // ??? Overwrites the corner speed. May need separate variable. - pl_prep_block->millimeters -= distance; // Potential round-off error near end of block. - pl_prep_block->millimeters = max(0.0,pl_prep_block->millimeters); // Shouldn't matter. - - // Update stepper block variables. - st_prep_data->step_events_remaining -= st_prep_block->n_step; - if ( st_prep_data->step_events_remaining == 0 ) { - // Move planner pointer to next block - st_prep_block->flag = ST_END_OF_BLOCK; - pl_prep_index = next_block_index(pl_prep_index); - pl_prep_block = NULL; - } else { - st_prep_block->flag = ST_NOOP; - } - - // New step block completed. Increment step buffer indices. - segment_buffer_head = segment_next_head; - segment_next_head = next_block_index(segment_buffer_head); - - } -} From f7429ec79b621d1a4fb4486eac5bd03f6258589e Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Thu, 24 Oct 2013 22:12:13 -0600 Subject: [PATCH 16/73] Cleaned up stepper and planner code. - Added some compile-time error checking. Will add more in future pushes to ensure settings are correct and within parameters that won't break anything. - Pushed some master branch changes with MEGA pin settings. - Cleaned up planner code and comments to clarify some of the new changes. Still much to do here. - Cleaned up the new stepper code. May need to abstract some of the segment buffer more to fix the feed holds (and integrate homing into the main stepper routine). With what's planned, this should make the stepper algorithm easier to attach other types of processes to it, where it is now tightly integrated with the planner buffer and nothing else. --- README.md | 2 +- config.h | 24 +++-- pin_map.h | 65 ++++++------ planner.c | 300 +++++++++++++++++++++++++++--------------------------- planner.h | 2 + stepper.c | 112 +++++++++----------- 6 files changed, 252 insertions(+), 253 deletions(-) diff --git a/README.md b/README.md index 660a778..e035870 100644 --- a/README.md +++ b/README.md @@ -11,7 +11,7 @@ Grbl includes full acceleration management with look ahead. That means the contr ##Changelog for v0.9 from v0.8 - **ALPHA status: Under heavy development.** - - New stepper algorithm: Based on the Pramod Ranade inverse time algorithm, but modified to ensure steps are executed exactly. This algorithm performs a constant timer tick and has a hard limit of 30kHz maximum step frequency. It is also highly tuneable and should be very easy to port to other microcontroller architectures. Overall, a much better, smoother stepper algorithm with the capability of very high speeds. + - New stepper algorithm: Based on an inverse time algorithm, but modified to ensure steps are executed exactly. This algorithm performs a constant timer tick and has a hard limit of 30kHz maximum step frequency. It is also highly tuneable and should be very easy to port to other microcontroller architectures. Overall, a much better, smoother stepper algorithm with the capability of very high speeds. - Planner optimizations: Multiple changes to increase planner execution speed and removed redundant variables. - Acceleration independence: Each axes may be defined with different acceleration parameters and Grbl will automagically calculate the maximum acceleration through a path depending on the direction traveled. This is very useful for machine that have very different axes properties, like the ShapeOko z-axis. - Maximum velocity independence: As with acceleration, the maximum velocity of individual axes may be defined. All seek/rapids motions will move at these maximum rates, but never exceed any one axes. So, when two or more axes move, the limiting axis will move at its maximum rate, while the other axes are scaled down. diff --git a/config.h b/config.h index f8f0810..8277046 100644 --- a/config.h +++ b/config.h @@ -49,9 +49,9 @@ #define CMD_CYCLE_START '~' #define CMD_RESET 0x18 // ctrl-x -// 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. Recommended step frequencies are limited by the Ranade frequency by +// The "Stepper Driver Interrupt" employs an inverse time algorithm to manage the Bresenham line +// stepping algorithm. The value ISR_TICKS_PER_SECOND is the frequency(Hz) at which the inverse time +// algorithm ticks at. Recommended step frequencies are limited by the inverse time frequency by // approximately 0.75-0.9 * ISR_TICK_PER_SECOND. Meaning for 30kHz, the max step frequency is roughly // 22.5-27kHz, but 30kHz is still possible, just not optimal. An Arduino can safely complete a single // interrupt of the current stepper driver algorithm theoretically up to a frequency of 35-40kHz, but @@ -71,7 +71,7 @@ // NOTE: Make sure this value is less than 256, when adjusting both dependent parameters. #define ISR_TICKS_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 +// The inverse time 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 // are finite so select the multiplier value high enough to avoid any numerical round-off issues and @@ -82,10 +82,10 @@ #define INV_TIME_MULTIPLIER 10000000.0 // Minimum stepper rate for the "Stepper Driver Interrupt". Sets the absolute minimum stepper rate -// in the stepper program and never runs slower than this value. If the RANADE_MULTIPLIER value +// in the stepper program and never runs slower than this value. If the INVE_TIME_MULTIPLIER value // changes, it will affect how this value works. So, if a zero is add/subtracted from the -// RANADE_MULTIPLIER value, do the same to this value if you want to same response. -// NOTE: Compute by (desired_step_rate/60) * RANADE_MULTIPLIER/ISR_TICKS_PER_SECOND. (mm/min) +// INV_TIME_MULTIPLIER value, do the same to this value if you want to same response. +// NOTE: Compute by (desired_step_rate/60) * INV_TIME_MULTIPLIER/ISR_TICKS_PER_SECOND. (mm/min) #define MINIMUM_STEP_RATE 1000L // Integer (mult*mm/isr_tic) // Minimum stepper rate. Only used by homing at this point. May be removed in later releases. @@ -158,7 +158,7 @@ // available RAM, like when re-compiling for a Mega or Sanguino. Or decrease if the Arduino // begins to crash due to the lack of available RAM or if the CPU is having trouble keeping // up with planning new incoming motions as they are executed. -// #define BLOCK_BUFFER_SIZE 17 // Uncomment to override default in planner.h. +// #define BLOCK_BUFFER_SIZE 18 // Uncomment to override default in planner.h. // Line buffer size from the serial input stream to be executed. Also, governs the size of // each of the startup blocks, as they are each stored as a string of this size. Make sure @@ -193,4 +193,12 @@ // TODO: Install compile-time option to send numeric status codes rather than strings. +// --------------------------------------------------------------------------------------- +// COMPILE-TIME ERROR CHECKING OF DEFINE VALUES: + +#if (ISR_TICKS_PER_ACCELERATION_TICK > 255) +#error Parameters ACCELERATION_TICKS / ISR_TICKS must be < 256 to prevent integer overflow. +#endif + +// --------------------------------------------------------------------------------------- #endif diff --git a/pin_map.h b/pin_map.h index 0167cc9..233b32a 100644 --- a/pin_map.h +++ b/pin_map.h @@ -97,41 +97,44 @@ #endif -#ifdef PIN_MAP_ARDUINO_MEGA_2560 // Unsupported. Doesn't work. Supplied by @elmom. +#ifdef PIN_MAP_ARDUINO_MEGA_2560 // Working @EliteEng // Serial port pins - #define SERIAL_RX USART0_RX_vect - #define SERIAL_UDRE USART0_UDRE_vect + #define SERIAL_RX USART0_RX_vect + #define SERIAL_UDRE USART0_UDRE_vect + + // Increase Buffers to make use of extra SRAM + #define RX_BUFFER_SIZE 256 + #define TX_BUFFER_SIZE 128 + #define BLOCK_BUFFER_SIZE 36 + #define LINE_BUFFER_SIZE 100 // NOTE: All step bit and direction pins must be on the same port. #define STEPPING_DDR DDRA #define STEPPING_PORT PORTA #define STEPPING_PIN PINA - #define X_STEP_BIT 0 // MEGA2560 Digital Pin 22 - #define Y_STEP_BIT 1 // MEGA2560 Digital Pin 23 - #define Z_STEP_BIT 2 // MEGA2560 Digital Pin 24 - // #define C_STEP_BIT 3 // MEGA2560 Digital Pin 25 - #define X_DIRECTION_BIT 4 // MEGA2560 Digital Pin 26 - #define Y_DIRECTION_BIT 5 // MEGA2560 Digital Pin 27 - #define Z_DIRECTION_BIT 6 // MEGA2560 Digital Pin 28 - // #define C_DIRECTION_BIT 7 // MEGA2560 Digital Pin 29 + #define X_STEP_BIT 2 // MEGA2560 Digital Pin 24 + #define Y_STEP_BIT 3 // MEGA2560 Digital Pin 25 + #define Z_STEP_BIT 4 // MEGA2560 Digital Pin 26 + #define X_DIRECTION_BIT 5 // MEGA2560 Digital Pin 27 + #define Y_DIRECTION_BIT 6 // MEGA2560 Digital Pin 28 + #define Z_DIRECTION_BIT 7 // MEGA2560 Digital Pin 29 #define STEP_MASK ((1<nominal_speed / \ @@ -119,55 +115,41 @@ void plan_update_partial_block(uint8_t block_index, float exit_speed_sqr) +-------------+ time --> - Recalculates the motion plan according to the following algorithm: + Recalculates the motion plan according to the following basic guidelines: - 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 speed is equal to or less than the maximum junction speed limit - b. No speed reduction within one block requires faster deceleration than the acceleration limits. - c. The last (or newest appended) block is planned from a complete stop. + 1. Go over every feasible block sequentially in reverse order and calculate the junction speeds + (i.e. current->entry_speed) such that: + a. No junction speed exceeds the pre-computed maximum junction speed limit or nominal speeds of + neighboring blocks. + b. A block entry speed cannot exceed one reverse-computed from its exit speed (next->entry_speed) + with a maximum allowable deceleration over the block travel distance. + c. The last (or newest appended) block is planned from a complete stop (an exit speed of zero). 2. Go over every block in chronological (forward) order and dial down junction speed values if - a. The speed increase within one block would require faster acceleration than the acceleration limits. + a. The exit speed exceeds the one forward-computed from its entry speed with the maximum allowable + acceleration over the block travel distance. - When these stages are complete, all blocks have a junction entry speed that will allow all speed changes - to be performed using the overall limiting acceleration value, and where no junction speed is greater - than the max limit. In other words, it just computed the fastest possible velocity profile through all - buffered blocks, where the final buffered block is planned to come to a full stop when the buffer is fully - executed. Finally it will: + When these stages are complete, the planner will have maximized the velocity profiles throughout the all + of the planner blocks, where every block is operating at its maximum allowable acceleration limits. In + other words, for all of the blocks in the planner, the plan is optimal and no further speed improvements + are possible. If a new block is added to the buffer, the plan is recomputed according to the said + guidelines for a new optimal plan. - 3. Convert the plan to data that the stepper algorithm needs. Only block trapezoids adjacent to a - a planner-modified junction speed with be updated, the others are assumed ok as is. + To increase computational efficiency of these guidelines, a set of planner block pointers have been + created to indicate stop-compute points for when the planner guidelines cannot logically make any further + changes or improvements to the plan when in normal operation and new blocks are streamed and added to the + planner buffer. For example, if a subset of sequential blocks in the planner have been planned and are + bracketed by junction velocities at their maximums (or by the first planner block as well), no new block + added to the planner buffer will alter the velocity profiles within them. So we no longer have to compute + them. Or, if a set of sequential blocks from the first block in the planner (or a optimal stop-compute + point) are all accelerating, they are all optimal and can not be altered by a new block added to the + planner buffer, as this will only further increase the plan speed to chronological blocks until a maximum + junction velocity is reached. However, if the operational conditions of the plan changes from infrequently + used feed holds or feedrate overrides, the stop-compute pointers will be reset and the entire plan is + recomputed as stated in the general guidelines. - All planner computations(1)(2) are performed in floating point to minimize numerical round-off errors. Only - when planned values are converted to stepper rate parameters(3), these are integers. If another motion block - is added while executing, the planner will re-plan and update the stored optimal velocity profile as it goes. - - Conceptually, the planner works like blowing up a balloon, where the balloon is the velocity profile. It's - constrained by the speeds at the beginning and end of the buffer, along with the maximum junction speeds and - nominal speeds of each block. Once a plan is computed, or balloon filled, this is the optimal velocity profile - through all of the motions in the buffer. Whenever a new block is added, this changes some of the limiting - conditions, or how the balloon is filled, so it has to be re-calculated to get the new optimal velocity profile. - - Also, since the planner only computes on what's in the planner buffer, some motions with lots of short line - segments, like arcs, may seem to move slow. This is because there simply isn't enough combined distance traveled - in the entire buffer to accelerate up to the nominal speed and then decelerate to a stop at the end of the - buffer. There are a few simple solutions to this: (1) Maximize the machine acceleration. The planner will be - able to compute higher speed profiles within the same combined distance. (2) Increase line segment(s) distance. - The more combined distance the planner has to use, the faster it can go. (3) Increase the MINIMUM_JUNCTION_SPEED. - Not recommended. This will change what speed the planner plans to at the end of the buffer. Can lead to lost - steps when coming to a stop. (4) [BEST] Increase the planner buffer size. The more combined distance, the - bigger the balloon, or faster it can go. But this is not possible for 328p Arduinos because its limited memory - is already maxed out. Future ARM versions should not have this issue, with look-ahead planner blocks numbering - up to a hundred or more. - - NOTE: Since this function is constantly re-calculating for every new incoming block, it must be as efficient - as possible. For example, in situations like arc generation or complex curves, the short, rapid line segments - can execute faster than new blocks can be added, and the planner buffer will then starve and empty, leading - to weird hiccup-like jerky motions. - - Index mapping: - - block_buffer_head: Points to the newest incoming buffer block just added by plan_buffer_line(). The planner - never touches the exit speed of this block, which always defaults to MINIMUM_JUNCTION_SPEED. + Planner buffer index mapping: + - block_buffer_head: Points to the newest incoming buffer block just added by plan_buffer_line(). The + planner never touches the exit speed of this block, which always defaults to 0. - block_buffer_tail: Points to the beginning of the planner buffer. First to be executed or being executed. Can dynamically change with the old stepper algorithm, but with the new algorithm, this should be impossible as long as the segment buffer is not empty. @@ -193,71 +175,47 @@ void plan_update_partial_block(uint8_t block_index, float exit_speed_sqr) entry speed. !!! Need to check if this is the start of the non-optimal or the end of the optimal block. + + + NOTE: All planner computations are performed in floating point to minimize numerical round-off errors. + When a planner block is executed, the floating point values are converted to fast integers by the stepper + algorithm segment buffer. See the stepper module for details. + + NOTE: Since the planner only computes on what's in the planner buffer, some motions with lots of short + line segments, like G2/3 arcs or complex curves, may seem to move slow. This is because there simply isn't + enough combined distance traveled in the entire buffer to accelerate up to the nominal speed and then + decelerate to a complete stop at the end of the buffer, as stated by the guidelines. If this happens and + becomes an annoyance, there are a few simple solutions: (1) Maximize the machine acceleration. The planner + will be able to compute higher velocity profiles within the same combined distance. (2) Maximize line + segment(s) distance per block to a desired tolerance. The more combined distance the planner has to use, + the faster it can go. (3) Maximize the planner buffer size. This also will increase the combined distance + for the planner to compute over. It also increases the number of computations the planner has to perform + to compute an optimal plan, so select carefully. The Arduino 328p memory is already maxed out, but future + ARM versions should have enough memory and speed for look-ahead blocks numbering up to a hundred or more. + */ static void planner_recalculate() { + + // Initialize block index to the last block in the planner buffer. + uint8_t block_index = prev_block_index(block_buffer_head); + // Query stepper module for safe planner block index to recalculate to, which corresponds to the end // of the step segment buffer. uint8_t block_buffer_safe = st_get_prep_block_index(); + // TODO: Make sure that we don't have to check for the block_buffer_tail condition, if the stepper module // returns a NULL pointer or something. This could happen when the segment buffer is empty. Although, // this call won't return a NULL, only an index.. I have to make sure that this index is synced with the // planner at all times. - - /* - In theory, the state of the segment buffer can exist anywhere within the planner buffer tail and head-1 - or is empty, when there is nothing in the segment queue. The safe pointer can be the buffer head only - when the planner queue has been entirely queued into the segment buffer and there are no more blocks - in the planner buffer. The segment buffer will to continue to execute the remainder of it, but the - planner should be able to treat a newly added block during this time as an empty planner buffer since - we can't touch the segment buffer. - - - The segment buffer is atomic to the planner buffer, because the main program computes these seperately. - Even if we move the planner head pointer early at the end of plan_buffer_line(), this shouldn't - effect the safe pointer. - - - If the safe pointer is at head-1, this means that the stepper algorithm has segments queued and may - be executing. This is the last block in the planner queue, so it has been planned to decelerate to - zero at its end. When adding a new block, there will be at least two blocks to work with. When resuming, - from a feed hold, we only have this block and will be computing nothing. The planner doesn't have to - do anything, since the trapezoid calculations called by the stepper module should complete the block plan. - - - In most cases, the safe pointer is at the plan tail or the block after, and rarely on the block two - beyond the tail. Since the safe pointer points to the block used at the end of the segment buffer, it - can be in any one of these states. As the stepper module executes the planner block, the buffer tail, - and hence the safe pointer, can push forward through the planner blocks and overcome the planned - pointer at any time. - - - Does the reverse pass not touch either the safe or the plan pointer blocks? The plan pointer only - allows the velocity profile within it to be altered, but not the entry speed, so the reverse pass - ignores this block. The safe pointer is the same way, where the entry speed does not change, but - the velocity profile within it does. - - - The planned pointer can exist anywhere in a given plan, except for the planner buffer head, if everything - operates as anticipated. Since the planner buffer can be executed by the stepper algorithm as any - rate and could empty the planner buffer quickly, the planner tail can overtake the planned pointer - at any time, but will never go around the ring buffer and re-encounter itself, the plan itself is not - changed by adding a new block or something else. - - - The planner recalculate function should always reset the planned pointer at the proper break points - or when it encounters the safe block pointer, but will only do so when there are more than one block - in the buffer. In the case of single blocks, the planned pointer should always be set to the first - write-able block in the buffer, aka safe block. - - - When does this not work? There might be an issue when the planned pointer moves from the tail to the - next head as a new block is being added and planned. Otherwise, the planned pointer should remain - static within the ring buffer no matter what the buffer is doing: being executed, adding new blocks, - or both simultaneously. Need to make sure that this case is covered. - */ - - + // Recompute plan only when there is more than one planner block in the buffer. Can't do anything with one. - // NOTE: block_buffer_safe can be equal to block_buffer_head if the segment buffer has completely queued up - // the remainder of the planner buffer. In this case, a new planner block will be treated as a single block. - if (block_buffer_head == block_buffer_safe) { // Also catches head = tail + // NOTE: block_buffer_safe can be the last planner block if the segment buffer has completely queued up the + // remainder of the planner buffer. In this case, a new planner block will be treated as a single block. + if (block_index == block_buffer_safe) { // Also catches (head-1) = tail // Just set block_buffer_planned pointer. - block_buffer_planned = block_buffer_head; - printString("z"); + block_buffer_planned = block_index; // TODO: Feedrate override of one block needs to update the partial block with an exit speed of zero. For // a single added block and recalculate after a feed hold, we don't need to compute this, since we already @@ -271,7 +229,6 @@ static void planner_recalculate() // all junctions before proceeding. // Initialize planner buffer pointers and indexing. - uint8_t block_index = block_buffer_head; plan_block_t *current = &block_buffer[block_index]; // Calculate maximum entry speed for last block in buffer, where the exit speed is always zero. @@ -285,7 +242,7 @@ static void planner_recalculate() // will be recomputed within the plan. So, we need to update it if it is partially completed. float entry_speed_sqr; plan_block_t *next; - block_index = prev_block_index(block_index); + block_index = plan_prev_block_index(block_index); if (block_index == block_buffer_safe) { // !! OR plan pointer? Yes I think so. @@ -294,7 +251,6 @@ static void planner_recalculate() // !!! Need to make the current entry speed calculation after this. plan_update_partial_block(block_index, 0.0); block_buffer_planned = block_index; -printString("y"); } else { @@ -306,7 +262,7 @@ printString("y"); // Increment block index early to check if the safe block is before the current block. If encountered, // this is an exit condition as we can't go further than this block in the reverse pass. - block_index = prev_block_index(block_index); + block_index = plan_prev_block_index(block_index); if (block_index == block_buffer_safe) { // Check if the safe block is partially completed. If so, update it before its exit speed // (=current->entry speed) is over-written. @@ -314,7 +270,7 @@ printString("y"); // the previous nominal speed to update this block with. There will need to be something along the // lines of a nominal speed change check and send the correct value to this function. plan_update_partial_block(block_index,current->entry_speed_sqr); -printString("x"); + // Set planned pointer at safe block and for loop exit after following computation is done. block_buffer_planned = block_index; } @@ -335,8 +291,8 @@ printString("x"); // Forward Pass: Forward plan the acceleration curve from the planned pointer onward. // Also scans for optimal plan breakpoints and appropriately updates the planned pointer. next = &block_buffer[block_buffer_planned]; // Begin at buffer planned pointer - block_index = next_block_index(block_buffer_planned); - while (block_index != next_buffer_head) { + block_index = plan_next_block_index(block_buffer_planned); + while (block_index != block_buffer_head) { current = next; next = &block_buffer[block_index]; @@ -363,11 +319,63 @@ printString("x"); block_buffer_planned = block_index; // Set optimal plan pointer } - block_index = next_block_index( block_index ); + block_index = plan_next_block_index( block_index ); } } +/* + uint8_t block_buffer_safe = st_get_prep_block_index(); + if (block_buffer_head == block_buffer_safe) { // Also catches head = tail + block_buffer_planned = block_buffer_head; + } else { + uint8_t block_index = block_buffer_head; + plan_block_t *current = &block_buffer[block_index]; + current->entry_speed_sqr = min( current->max_entry_speed_sqr, 2*current->acceleration*current->millimeters); + float entry_speed_sqr; + plan_block_t *next; + block_index = plan_prev_block_index(block_index); + if (block_index == block_buffer_safe) { // !! OR plan pointer? Yes I think so. + plan_update_partial_block(block_index, 0.0); + block_buffer_planned = block_index; + } else { + while (block_index != block_buffer_planned) { + next = current; + current = &block_buffer[block_index]; + block_index = plan_prev_block_index(block_index); + if (block_index == block_buffer_safe) { + plan_update_partial_block(block_index,current->entry_speed_sqr); + block_buffer_planned = block_index; + } + if (current->entry_speed_sqr != current->max_entry_speed_sqr) { + entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters; + if (entry_speed_sqr < current->max_entry_speed_sqr) { + current->entry_speed_sqr = entry_speed_sqr; + } else { + current->entry_speed_sqr = current->max_entry_speed_sqr; + } + } + } + + } + next = &block_buffer[block_buffer_planned]; // Begin at buffer planned pointer + block_index = plan_next_block_index(block_buffer_planned); + while (block_index != next_buffer_head) { + current = next; + next = &block_buffer[block_index]; + if (current->entry_speed_sqr < next->entry_speed_sqr) { + entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters; + if (entry_speed_sqr < next->entry_speed_sqr) { + next->entry_speed_sqr = entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass sets this. + block_buffer_planned = block_index; // Set optimal plan pointer. + } + } + if (next->entry_speed_sqr == next->max_entry_speed_sqr) { + block_buffer_planned = block_index; // Set optimal plan pointer + } + block_index = plan_next_block_index( block_index ); + } + } */ } @@ -376,11 +384,12 @@ void plan_reset_buffer() block_buffer_planned = block_buffer_tail; } + void plan_init() { block_buffer_tail = 0; block_buffer_head = 0; // Empty = tail - next_buffer_head = 1; // next_block_index(block_buffer_head) + next_buffer_head = 1; // plan_next_block_index(block_buffer_head) plan_reset_buffer(); memset(&pl, 0, sizeof(pl)); // Clear planner struct } @@ -389,7 +398,7 @@ void plan_init() void plan_discard_current_block() { if (block_buffer_head != block_buffer_tail) { // Discard non-empty buffer. - block_buffer_tail = next_block_index( block_buffer_tail ); + block_buffer_tail = plan_next_block_index( block_buffer_tail ); } } @@ -484,10 +493,8 @@ void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) if (feed_rate < 0) { feed_rate = SOME_LARGE_VALUE; } // Scaled down to absolute max/rapids rate later else if (invert_feed_rate) { feed_rate = block->millimeters/feed_rate; } - // Calculate the unit vector of the line move and the block maximum feed rate and acceleration limited - // by the maximum possible values. Block rapids rates are computed or feed rates are scaled down so - // they don't exceed the maximum axes velocities. The block acceleration is maximized based on direction - // and axes properties as well. + // Calculate the unit vector of the line move and the block maximum feed rate and acceleration scaled + // down such that no individual axes maximum values are exceeded with respect to the line direction. // NOTE: This calculation assumes all axes are orthogonal (Cartesian) and works with ABC-axes, // if they are also orthogonal/independent. Operates on the absolute value of the unit vector. float inverse_unit_vec_value; @@ -514,10 +521,6 @@ void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) if (block_buffer_head == block_buffer_tail) { // Initialize block entry speed as zero. Assume it will be starting from rest. Planner will correct this later. - // !!! Ensures when the first block starts from zero speed. If we do this in the planner, this will break - // feedrate overrides later, as you can override this single block and it maybe moving already at a given rate. - // Better to do it here and make it clean. - // !!! Shouldn't need this for anything other than a single block. block->entry_speed_sqr = 0.0; block->max_junction_speed_sqr = 0.0; // Starting from rest. Enforce start from zero velocity. @@ -541,10 +544,9 @@ void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) a continuous mode path, but ARM-based microcontrollers most certainly do. NOTE: The max junction speed is a fixed value, since machine acceleration limits cannot be - changed dynamically during operation nor can the line segment geometry. This must be kept in + changed dynamically during operation nor can the line move geometry. This must be kept in memory in the event of a feedrate override changing the nominal speeds of blocks, which can change the overall maximum entry speed conditions of all blocks. - */ // NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta). float sin_theta_d2 = sqrt(0.5*(1.0-junction_cos_theta)); // Trig half angle identity. Always positive. @@ -558,7 +560,6 @@ void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) block->nominal_speed_sqr = feed_rate*feed_rate; // (mm/min). Always > 0 // Compute the junction maximum entry based on the minimum of the junction speed and neighboring nominal speeds. - // TODO: Should call a function to determine this. The function can be used elsewhere for feedrate overrides later. block->max_entry_speed_sqr = min(block->max_junction_speed_sqr, min(block->nominal_speed_sqr,pl.previous_nominal_speed_sqr)); @@ -569,17 +570,16 @@ void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) // Update planner position memcpy(pl.position, target_steps, sizeof(target_steps)); // pl.position[] = target_steps[] - planner_recalculate(); - - // Update buffer head and next buffer head indices. Advance only after new plan has been computed. + // New block is all set. Update buffer head and next buffer head indices. block_buffer_head = next_buffer_head; - next_buffer_head = next_block_index(block_buffer_head); + next_buffer_head = plan_next_block_index(block_buffer_head); + // Finish up by recalculating the plan with the new block. + planner_recalculate(); - -int32_t blength = block_buffer_head - block_buffer_tail; -if (blength < 0) { blength += BLOCK_BUFFER_SIZE; } -printInteger(blength); +// int32_t blength = block_buffer_head - block_buffer_tail; +// if (blength < 0) { blength += BLOCK_BUFFER_SIZE; } +// printInteger(blength); } @@ -595,36 +595,34 @@ void plan_sync_position() } - - /* STEPPER VELOCITY PROFILE DEFINITION - less than nominal rate-> + - +--------+ <- nominal_rate /|\ + less than nominal speed-> + + +--------+ <- nominal_speed /|\ / \ / | \ - initial_rate -> + \ / | + <- next->initial_rate - | + <- next->initial_rate / | | - +-------------+ initial_rate -> +----+--+ + entry_speed -> + \ / | + <- next->entry_speed + | + <- next->entry_speed / | | + +-------------+ entry_speed -> +----+--+ time --> ^ ^ ^ ^ | | | | decelerate distance decelerate distance - Calculates the "trapezoid" velocity profile parameters of a planner block for the stepper - algorithm. The planner computes the entry and exit speeds of each block, but does not bother to - determine the details of the velocity profiles within them, as they aren't needed for computing - an optimal plan. When the stepper algorithm begins to execute a block, the block velocity profiles - are computed ad hoc. + Calculates the type of velocity profile for a given planner block and provides the deceleration + distance for the stepper algorithm to use to accurately trace the profile exactly. The planner + computes the entry and exit speeds of each block, but does not bother to determine the details of + the velocity profiles within them, as they aren't needed for computing an optimal plan. When the + stepper algorithm begins to execute a block, the block velocity profiles are computed ad hoc. Each block velocity profiles can be described as either a trapezoidal or a triangular shape. The trapezoid occurs when the block reaches the nominal speed of the block and cruises for a period of time. A triangle occurs when the nominal speed is not reached within the block. Both of these velocity profiles may also be truncated on either end with no acceleration or deceleration ramps, - as they can be influenced by the conditions of neighboring blocks. + as they can be influenced by the conditions of neighboring blocks, where the acceleration ramps + are defined by constant acceleration equal to the maximum allowable acceleration of a block. - The following function determines the type of velocity profile and stores the minimum required - information for the stepper algorithm to execute the calculated profiles. Since the stepper - algorithm always assumes to begin accelerating from the initial_rate and cruise if the nominal_rate - is reached, we only need to know when to begin deceleration to the end of the block. Hence, only - the distance from the end of the block to begin a deceleration ramp are computed. + Since the stepper algorithm already assumes to begin executing a planner block by accelerating + from the planner entry speed and cruise if the nominal speed is reached, we only need to know + when to begin deceleration to the end of the block. Hence, only the distance from the end of the + block to begin a deceleration ramp is computed for the stepper algorithm when requested. */ float plan_calculate_velocity_profile(uint8_t block_index) { @@ -632,7 +630,7 @@ float plan_calculate_velocity_profile(uint8_t block_index) // Determine current block exit speed float exit_speed_sqr = 0.0; // Initialize for end of planner buffer. Zero speed. - plan_block_t *next_block = plan_get_block_by_index(next_block_index(block_index)); + plan_block_t *next_block = plan_get_block_by_index(plan_next_block_index(block_index)); if (next_block != NULL) { exit_speed_sqr = next_block->entry_speed_sqr; } // Exit speed is the entry speed of next buffer block // First determine intersection distance (in steps) from the exit point for a triangular profile. diff --git a/planner.h b/planner.h index 083d182..a52a4f5 100644 --- a/planner.h +++ b/planner.h @@ -64,6 +64,8 @@ void plan_discard_current_block(); // Gets the current block. Returns NULL if buffer empty plan_block_t *plan_get_current_block(); +uint8_t plan_next_block_index(uint8_t block_index); + plan_block_t *plan_get_block_by_index(uint8_t block_index); float plan_calculate_velocity_profile(uint8_t block_index); diff --git a/stepper.c b/stepper.c index 27cf7d6..de93000 100644 --- a/stepper.c +++ b/stepper.c @@ -115,23 +115,6 @@ static uint8_t st_data_prep_index; // Index of stepper common data block bein static uint8_t pl_partial_block_flag; // Flag indicating the planner has modified the prepped planner block -// Returns the index of the next block in the ring buffer -// NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication. -static uint8_t next_block_index(uint8_t block_index) -{ - block_index++; - if (block_index == SEGMENT_BUFFER_SIZE) { block_index = 0; } - return(block_index); -} - -static uint8_t next_block_pl_index(uint8_t block_index) -{ - block_index++; - if (block_index == BLOCK_BUFFER_SIZE) { block_index = 0; } - return(block_index); -} - - /* __________________________ /| |\ _________________ ^ / | | \ /| |\ | @@ -334,9 +317,7 @@ ISR(TIMER2_COMPA_vect) st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter if (st.delta_d > st_current_data->rate_delta) { st.delta_d -= st_current_data->rate_delta; - } else { - - // Moving near zero feed rate. Gracefully slow down. + } else { // Moving near zero feed rate. Gracefully slow down. st.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow. // TODO: Check for and handle feed hold exit? At this point, machine is stopped. @@ -392,16 +373,16 @@ ISR(TIMER2_COMPA_vect) // Check step events for trapezoid change or end of block. st.segment_steps_remaining--; // Decrement step events count if (st.segment_steps_remaining == 0) { - - // NOTE: sys.position updates could be done here. The bresenham counters can have - // their own fast 8-bit addition-only counters. Here we would check the direction and - // apply it to sys.position accordingly. However, this could take too much time - // combined with loading a new segment during next cycle too. - // TODO: Measure the time it would take in the worst case. It could still be faster - // overall during segment execution if uint8 step counters tracked this and was added - // to the system position variables here. Compared to worst case now, it wouldn't be - // that much different. /* + NOTE: sys.position updates could be done here. The bresenham counters can have + their own fast 8-bit addition-only counters. Here we would check the direction and + apply it to sys.position accordingly. However, this could take too much time + combined with loading a new segment during next cycle too. + TODO: Measure the time it would take in the worst case. It could still be faster + overall during segment execution if uint8 step counters tracked this and was added + to the system position variables here. Compared to worst case now, it wouldn't be + that much different. + // TODO: Upon loading, step counters would need to be zeroed. // TODO: For feedrate overrides, we will have to execute add these values.. although // for probing, this breaks. Current values won't be correct, unless we query it. @@ -429,8 +410,8 @@ ISR(TIMER2_COMPA_vect) st.load_flag = LOAD_SEGMENT; } - // Discard current segment - segment_buffer_tail = next_block_index( segment_buffer_tail ); + // Discard current segment by advancing buffer tail index + if ( ++segment_buffer_tail == SEGMENT_BUFFER_SIZE) { segment_buffer_tail = 0; } } @@ -552,28 +533,32 @@ void st_cycle_reinitialize() /* Prepares step segment buffer. Continuously called from main program. - NOTE: There doesn't seem to be a great way to figure out how many steps occur within - a set number of ISR ticks. Numerical round-off and CPU overhead always seems to be a - critical problem. So, either numerical round-off checks could be made to account for - them, while CPU overhead could be minimized in some way, or we can flip the algorithm - around to have the stepper algorithm track number of steps over an indeterminant amount - of time instead. - In other words, we use the planner velocity floating point data to get an estimate of - the number of steps we want to execute. We then back out the approximate velocity for - the planner to use, which should be much more robust to round-off error. The main problem - now is that we are loading the stepper algorithm to handle acceleration now, rather than - pre-calculating with the main program. This approach does make sense in the way that - planner velocities and stepper profiles can be traced more accurately. - Which is better? Very hard to tell. The time-based algorithm would be able to handle - Bresenham step adaptive-resolution much easier and cleaner. Whereas, the step-based would - require some additional math in the stepper algorithm to adjust on the fly, plus adaptation - would occur in a non-deterministic manner. - I suppose it wouldn't hurt to build both to see what's better. Just a lot more work. + The segment buffer is an intermediary buffer interface between the execution of steps + by the stepper algorithm and the velocity profiles generated by the planner. The stepper + algorithm only executes steps within the segment buffer and is filled by the main program + when steps are "checked-out" from the first block in the planner buffer. This keeps the + step execution and planning optimization processes atomic and protected from each other. + The number of steps "checked-out" from the planner buffer and the number of segments in + the segment buffer is sized and computed such that no operation in the main program takes + longer than the time it takes the stepper algorithm to empty it before refilling it. + Currently, the segment buffer conservatively holds roughly up to 40-60 msec of steps. - TODO: Need to describe the importance of continuations of step pulses between ramp states - and planner blocks. This has to do with Alden's problem with step "phase". The things I've - been doing here limit this phase issue by truncating some of the ramp timing for certain - events like deceleration initialization and end of block. + NOTE: The segment buffer executes a set number of steps over an approximate time period. + If we try to execute over a set time period, it is difficult to guarantee or predict how + many steps will execute over it, especially when the step pulse phasing between the + neighboring segments are kept consistent. Meaning that, if the last segment step pulses + right before its end, the next segment must delay its first pulse so that the step pulses + are consistently spaced apart over time to keep the step pulse train nice and smooth. + Keeping track of phasing and ensuring that the exact number of steps are executed as + defined by the planner block, the related computational overhead gets quickly and + prohibitively expensive, especially in real-time. + Since the stepper algorithm automatically takes care of the step pulse phasing with + its ramp and inverse time counters, we don't have to explicitly and expensively track the + exact number of steps, time, or phasing of steps. All we need to do is approximate + the number of steps in each segment such that the segment buffer has enough execution time + for the main program to do what it needs to do and refill it when it has time. In other + words, we just need to compute a cheap approximation of the current velocity and the + number of steps over it. */ /* @@ -606,7 +591,7 @@ void st_prep_buffer() pl_prep_block = plan_get_block_by_index(pl_prep_index); // Query planner for a queued block if (pl_prep_block == NULL) { return; } // No planner blocks. Exit. - // Increment stepper common data buffer index + // Increment stepper common data index if ( ++st_data_prep_index == (SEGMENT_BUFFER_SIZE-1) ) { st_data_prep_index = 0; } // Check if the planner has re-computed this block mid-execution. If so, push the previous segment @@ -666,7 +651,10 @@ void st_prep_buffer() // Set new segment to point to the current segment data block. prep_segment->st_data_index = st_data_prep_index; - // Approximate the velocity over the new segment + // Approximate the velocity over the new segment using the already computed rate values. + // NOTE: This assumes that each segment will have an execution time roughly equal to every ACCELERATION_TICK. + // We do this to minimize memory and computational requirements. However, this could easily be replaced with + // a more exact approximation or have a unique time per segment, if CPU and memory overhead allows. if (st_prep_data->decelerate_after <= 0) { if (st_prep_data->decelerate_after == 0) { prep_segment->flag = SEGMENT_DECEL; } else { st_prep_data->current_approx_rate -= st_prep_data->rate_delta; } @@ -680,15 +668,15 @@ void st_prep_buffer() } } - // Compute the number of steps in the prepped segment based on the approximate current rate. The execution - // time of each segment should be about every ACCELERATION_TICK. + // Compute the number of steps in the prepped segment based on the approximate current rate. // NOTE: The d_next divide cancels out the INV_TIME_MULTIPLIER and converts the rate value to steps. - // NOTE: As long as the ACCELERATION_TICKS_PER_SECOND is valid, n_step should never exceed 255. prep_segment->n_step = ceil(max(MINIMUM_STEP_RATE,st_prep_data->current_approx_rate)* (ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND)/st_prep_data->d_next); - prep_segment->n_step = max(prep_segment->n_step,MINIMUM_STEPS_PER_SEGMENT); // Ensure it moves for very slow motions? + // NOTE: Ensures it moves for very slow motions, but MINIMUM_STEP_RATE should always set this too. Perhaps + // a compile-time check to see if MINIMUM_STEP_RATE is set high enough is all that is needed. + prep_segment->n_step = max(prep_segment->n_step,MINIMUM_STEPS_PER_SEGMENT); + // NOTE: As long as the ACCELERATION_TICKS_PER_SECOND is valid, n_step should never exceed 255 and overflow. // prep_segment->n_step = min(prep_segment->n_step,MAXIMUM_STEPS_PER_BLOCK); // Prevent unsigned int8 overflow. - // Check if n_step exceeds steps remaining in planner block. If so, truncate. if (prep_segment->n_step > st_prep_data->step_events_remaining) { @@ -703,7 +691,7 @@ void st_prep_buffer() } } - // Update stepper block variables. + // Update stepper common data variables. st_prep_data->decelerate_after -= prep_segment->n_step; st_prep_data->step_events_remaining -= prep_segment->n_step; @@ -712,13 +700,13 @@ void st_prep_buffer() // Set EOB bitflag so stepper algorithm discards the planner block after this segment completes. prep_segment->flag |= SEGMENT_END_OF_BLOCK; // Move planner pointer to next block and flag to load a new block for the next segment. - pl_prep_index = next_block_pl_index(pl_prep_index); + pl_prep_index = plan_next_block_index(pl_prep_index); pl_prep_block = NULL; } // New step segment completed. Increment segment buffer indices. segment_buffer_head = segment_next_head; - segment_next_head = next_block_index(segment_buffer_head); + if ( ++segment_next_head == SEGMENT_BUFFER_SIZE ) { segment_next_head = 0; } // long a = prep_segment->n_step; // printInteger(a); From 27297d444ba8480ee27327c6644c48485601e57d Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Tue, 29 Oct 2013 08:31:48 -0600 Subject: [PATCH 17/73] Updated comments. Changed stepper variable names to be more understandable. Added step locking note. - Updated config comments and stepper code comments for the new changes. - Changed stepper algorithm variable names to be more understandable in what they actually do. - Added a stepper lock note in default.h per user request. - Started some code layout in handling feed holds and refactoring the homing routine to use the main stepper algorithm instead of a seperate version. --- config.h | 13 +- defaults.h | 8 +- limits.c | 19 +- protocol.c | 2 + stepper.c | 164 +++++------- stepper_old.c | 691 +++++++++++++++++++++++++++++++++++++------------ stepper_v0_9.c | 387 +++++++++++++++++++++++++++ 7 files changed, 998 insertions(+), 286 deletions(-) create mode 100644 stepper_v0_9.c diff --git a/config.h b/config.h index 8277046..9aa82f0 100644 --- a/config.h +++ b/config.h @@ -71,12 +71,13 @@ // NOTE: Make sure this value is less than 256, when adjusting both dependent parameters. #define ISR_TICKS_PER_ACCELERATION_TICK (ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND) -// The inverse time 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 -// are finite so select the multiplier value high enough to avoid any numerical round-off issues and -// still have enough range to account for all motion types. However, in most all imaginable CNC -// applications, the following multiplier value will work more than well enough. If you do have +// The inverse time algorithm can use either floating point or long integers for its counters (usually +// very small values ~10^-6), but with integers, the counter values must be scaled to be greater than +// one. This multiplier value scales the floating point counter values for use in a long integer, which +// are significantly faster to compute with a slightly higher precision ceiling than floats. Long +// integers are finite so select the multiplier value high enough to avoid any numerical round-off +// issues and still have enough range to account for all motion types. However, in most all imaginable +// CNC applications, the following multiplier value will work more than well enough. If you do have // happened to weird stepper motion issues, try modifying this value by adding or subtracting a // zero and report it to the Grbl administrators. #define INV_TIME_MULTIPLIER 10000000.0 diff --git a/defaults.h b/defaults.h index d3ddc1c..7ee6606 100644 --- a/defaults.h +++ b/defaults.h @@ -50,7 +50,7 @@ #define DEFAULT_HOMING_FEEDRATE 25.0 // mm/min #define DEFAULT_HOMING_DEBOUNCE_DELAY 100 // msec (0-65k) #define DEFAULT_HOMING_PULLOFF 1.0 // mm - #define DEFAULT_STEPPER_IDLE_LOCK_TIME 25 // msec (0-255) + #define DEFAULT_STEPPER_IDLE_LOCK_TIME 25 // msec (0-254, 255 keeps steppers enabled) #define DEFAULT_DECIMAL_PLACES 3 #define DEFAULT_X_MAX_TRAVEL 200 // mm #define DEFAULT_Y_MAX_TRAVEL 200 // mm @@ -84,7 +84,7 @@ #define DEFAULT_HOMING_FEEDRATE 25.0 // mm/min #define DEFAULT_HOMING_DEBOUNCE_DELAY 100 // msec (0-65k) #define DEFAULT_HOMING_PULLOFF 1.0 // mm - #define DEFAULT_STEPPER_IDLE_LOCK_TIME 25 // msec (0-255) + #define DEFAULT_STEPPER_IDLE_LOCK_TIME 25 // msec (0-254, 255 keeps steppers enabled) #define DEFAULT_DECIMAL_PLACES 3 #define DEFAULT_X_MAX_TRAVEL 200 // mm #define DEFAULT_Y_MAX_TRAVEL 200 // mm @@ -121,7 +121,7 @@ #define DEFAULT_HOMING_FEEDRATE 25.0 // mm/min #define DEFAULT_HOMING_DEBOUNCE_DELAY 100 // msec (0-65k) #define DEFAULT_HOMING_PULLOFF 1.0 // mm - #define DEFAULT_STEPPER_IDLE_LOCK_TIME 255 // msec (0-255) + #define DEFAULT_STEPPER_IDLE_LOCK_TIME 255 // msec (0-254, 255 keeps steppers enabled) #define DEFAULT_DECIMAL_PLACES 3 #define DEFAULT_X_MAX_TRAVEL 200 // mm #define DEFAULT_Y_MAX_TRAVEL 200 // mm @@ -156,7 +156,7 @@ #define DEFAULT_HOMING_FEEDRATE 50.0 // mm/min #define DEFAULT_HOMING_DEBOUNCE_DELAY 100 // msec (0-65k) #define DEFAULT_HOMING_PULLOFF 1.0 // mm - #define DEFAULT_STEPPER_IDLE_LOCK_TIME 25 // msec (0-255) + #define DEFAULT_STEPPER_IDLE_LOCK_TIME 25 // msec (0-254, 255 keeps steppers enabled) #define DEFAULT_DECIMAL_PLACES 3 #define DEFAULT_X_MAX_TRAVEL 200 // mm #define DEFAULT_Y_MAX_TRAVEL 200 // mm diff --git a/limits.c b/limits.c index a03d32e..4fde4c9 100644 --- a/limits.c +++ b/limits.c @@ -59,12 +59,6 @@ void limits_init() // your e-stop switch to the Arduino reset pin, since it is the most correct way to do this. ISR(LIMIT_INT_vect) { - // TODO: This interrupt may be used to manage the homing cycle directly with the main stepper - // interrupt without adding too much to it. All it would need is some way to stop one axis - // when its limit is triggered and continue the others. This may reduce some of the code, but - // would make Grbl a little harder to read and understand down road. Holding off on this until - // we move on to new hardware or flash space becomes an issue. If it ain't broke, don't fix it. - // Ignore limit switches if already in an alarm state or in-process of executing an alarm. // When in the alarm state, Grbl should have been reset or will force a reset, so any pending // moves in the planner and serial buffers are all cleared and newly sent blocks will be @@ -89,6 +83,19 @@ ISR(LIMIT_INT_vect) // NOTE: Only the abort runtime command can interrupt this process. static void homing_cycle(uint8_t cycle_mask, int8_t pos_dir, bool invert_pin, float homing_rate) { + + /* TODO: Change homing routine to call planner instead moving at the maximum seek rates + and (max_travel+10mm?) for each axes during the search phase. The routine should monitor + the state of the limit pins and when a pin is triggered, it can disable that axes by + setting the respective step_x, step_y, or step_z value in the executing planner block. + This keeps the stepper algorithm counters from triggering the step on that particular + axis. When all axes have been triggered, we can then disable the steppers and reset + the stepper and planner buffers. This same method can be used for the locate cycles. + This will also fix the slow max feedrate of the homing 'lite' stepper algorithm. + + Need to check if setting the planner steps will require them to be volatile or not. */ + + // Determine governing axes with finest step resolution per distance for the Bresenham // algorithm. This solves the issue when homing multiple axes that have different // resolutions without exceeding system acceleration setting. It doesn't have to be diff --git a/protocol.c b/protocol.c index 253ec1f..5466720 100644 --- a/protocol.c +++ b/protocol.c @@ -104,7 +104,9 @@ ISR(PINOUT_INT_vect) // limit switches, or the main program. void protocol_execute_runtime() { + // Reload step segment buffer st_prep_buffer(); + if (sys.execute) { // Enter only if any bit flag is true uint8_t rt_exec = sys.execute; // Avoid calling volatile multiple times diff --git a/stepper.c b/stepper.c index de93000..22f1edd 100644 --- a/stepper.c +++ b/stepper.c @@ -55,9 +55,9 @@ typedef struct { uint8_t segment_steps_remaining; // Steps remaining in line segment motion // Used by inverse time algorithm to track step rate - int32_t counter_d; // Inverse time distance traveled since last step event - uint32_t delta_d; // Inverse time distance traveled per interrupt tick - uint32_t d_per_tick; + int32_t counter_dist; // Inverse time distance traveled since last step event + uint32_t ramp_rate; // Inverse time distance traveled per interrupt tick + uint32_t dist_per_tick; // Used by the stepper driver interrupt uint8_t execute_step; // Flags step execution for each interrupt. @@ -65,7 +65,7 @@ typedef struct { uint8_t out_bits; // The next stepping-bits to be output uint8_t load_flag; - uint8_t ramp_count; + uint8_t counter_ramp; uint8_t ramp_type; } stepper_t; static stepper_t st; @@ -76,7 +76,7 @@ static stepper_t st; // the number of accessible stepper buffer segments (SEGMENT_BUFFER_SIZE-1). typedef struct { int32_t step_events_remaining; // Tracks step event count for the executing planner block - uint32_t d_next; // Scaled distance to next step + uint32_t dist_next_step; // Scaled distance to next step uint32_t initial_rate; // Initialized step rate at re/start of a planner block uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute uint32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive) @@ -154,7 +154,6 @@ void st_wake_up() TCNT2 = 0; // Clear Timer2 TIMSK2 |= (1<n_step; - // Check if the counters need to be reset for a new planner block + // If the new segment starts a new planner block, initialize stepper variables and counters. + // NOTE: For new segments only, the step counters are not updated to ensure step phasing is continuous. if (st.load_flag == LOAD_BLOCK) { pl_current_block = plan_get_current_block(); // Should always be there. Stepper buffer handles this. - st_current_data = &segment_data[segment_buffer[segment_buffer_tail].st_data_index]; //st_current_segment->st_data_index]; + st_current_data = &segment_data[segment_buffer[segment_buffer_tail].st_data_index]; - // Initialize direction bits for block + // Initialize direction bits for block. Set execute flag to set directions bits upon next ISR tick. st.out_bits = pl_current_block->direction_bits ^ settings.invert_mask; - st.execute_step = true; // Set flag to set direction bits upon next ISR tick. + st.execute_step = true; // Initialize Bresenham line counters st.counter_x = (pl_current_block->step_event_count >> 1); st.counter_y = st.counter_x; st.counter_z = st.counter_x; - // Initialize inverse time and step rate counter data - st.counter_d = st_current_data->d_next; // d_next always greater than delta_d. - if (st.delta_d < MINIMUM_STEP_RATE) { st.d_per_tick = MINIMUM_STEP_RATE; } - else { st.d_per_tick = st.delta_d; } - - // During feed hold, do not update rate, ramp type, or ramp counters. Keep decelerating. -// if (sys.state == STATE_CYCLE) { - st.delta_d = st_current_data->initial_rate; - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Initialize ramp counter via midpoint rule - st.ramp_type = RAMP_NOOP_CRUISE; // Initialize as no ramp operation. Corrected later if necessary. -// } - + // Initialize inverse time, step rate data, and acceleration ramp counters + st.counter_dist = st_current_data->dist_next_step; // dist_next_step always greater than ramp_rate. + st.ramp_rate = st_current_data->initial_rate; + st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK/2; // Initialize ramp counter via midpoint rule + st.ramp_type = RAMP_NOOP_CRUISE; // Initialize as no ramp operation. Corrected later if necessary. + + // Ensure the initial step rate exceeds the MINIMUM_STEP_RATE. + if (st.ramp_rate < MINIMUM_STEP_RATE) { st.dist_per_tick = MINIMUM_STEP_RATE; } + else { st.dist_per_tick = st.ramp_rate; } } - // Acceleration and cruise handled by ramping. Just check if deceleration needs to begin. + // Check if ramp conditions have changed. If so, update ramp counters and control variables. if ( st_current_segment->flag & (SEGMENT_DECEL | SEGMENT_ACCEL) ) { /* Compute correct ramp count for a ramp change. Upon a switch from acceleration to deceleration, or vice-versa, the new ramp count must be set to trigger the next acceleration tick equal to the number of ramp ISR ticks counted since the last acceleration tick. This is ensures the ramp is executed exactly as the plan dictates. Otherwise, when a ramp begins from a known rate (nominal/cruise or initial), the ramp count must be set to ISR_TICKS_PER_ACCELERATION_TICK/2 - as mandated by the mid-point rule. For these conditions, the ramp count have been initialized - such that the following computation is still correct. */ - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; + as mandated by the mid-point rule. For the latter conditions, the ramp count have been + initialized such that the following computation is still correct. */ + st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK-st.counter_ramp; if ( st_current_segment->flag & SEGMENT_DECEL ) { st.ramp_type = RAMP_DECEL; } else { st.ramp_type = RAMP_ACCEL; } } @@ -300,46 +290,36 @@ ISR(TIMER2_COMPA_vect) } // Adjust inverse time counter for ac/de-celerations - // NOTE: Accelerations are handled by the stepper algorithm as it's thought to be more computationally - // efficient on the Arduino AVR. This could may not be true with higher ISR frequencies or faster CPUs. - if (st.ramp_type) { // Ignored when ramp type is NOOP_CRUISE - st.ramp_count--; // Tick acceleration ramp counter - if (st.ramp_count == 0) { // Adjust step rate when its time + if (st.ramp_type) { // Ignored when ramp type is RAMP_NOOP_CRUISE + st.counter_ramp--; // Tick acceleration ramp counter + if (st.counter_ramp == 0) { // Adjust step rate when its time + st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter if (st.ramp_type == RAMP_ACCEL) { // Adjust velocity for acceleration - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter - st.delta_d += st_current_data->rate_delta; - if (st.delta_d >= st_current_data->nominal_rate) { // Reached nominal rate. - st.delta_d = st_current_data->nominal_rate; // Set cruising velocity + st.ramp_rate += st_current_data->rate_delta; + if (st.ramp_rate >= st_current_data->nominal_rate) { // Reached nominal rate. + st.ramp_rate = st_current_data->nominal_rate; // Set cruising velocity st.ramp_type = RAMP_NOOP_CRUISE; // Set ramp flag to cruising - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Re-initialize counter for next ramp. + st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK/2; // Re-initialize counter for next ramp change. } } else { // Adjust velocity for deceleration. - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter - if (st.delta_d > st_current_data->rate_delta) { - st.delta_d -= st_current_data->rate_delta; + if (st.ramp_rate > st_current_data->rate_delta) { + st.ramp_rate -= st_current_data->rate_delta; } else { // Moving near zero feed rate. Gracefully slow down. - st.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow. - - // TODO: Check for and handle feed hold exit? At this point, machine is stopped. - // - Set system flag to recompute plan and reset segment buffer. - // - Segment steps in buffer needs to be returned to planner correctly. - // busy = false; - // return; - + st.ramp_rate >>= 1; // Integer divide by 2 until complete. Also prevents overflow. } } - // Finalize adjusted step rate. Ensure minimum. - if (st.delta_d < MINIMUM_STEP_RATE) { st.d_per_tick = MINIMUM_STEP_RATE; } - else { st.d_per_tick = st.delta_d; } + // Adjust for minimum step rate, but retain operating ramp rate for accurate velocity tracing. + if (st.ramp_rate < MINIMUM_STEP_RATE) { st.dist_per_tick = MINIMUM_STEP_RATE; } + else { st.dist_per_tick = st.ramp_rate; } } } // Iterate inverse time counter. Triggers each Bresenham step event. - st.counter_d -= st.d_per_tick; + st.counter_dist -= st.dist_per_tick; // Execute Bresenham step event, when it's time to do so. - if (st.counter_d < 0) { - st.counter_d += st_current_data->d_next; // Reload inverse time counter + if (st.counter_dist < 0) { + st.counter_dist += st_current_data->dist_next_step; // Reload inverse time counter st.out_bits = pl_current_block->direction_bits; // Reset out_bits and reload direction bits st.execute_step = true; @@ -349,7 +329,6 @@ ISR(TIMER2_COMPA_vect) if (st.counter_x < 0) { st.out_bits |= (1<step_event_count; - // st.steps_x++; if (st.out_bits & (1<step_event_count; - // st.steps_y++; if (st.out_bits & (1<step_event_count; - // st.steps_z++; if (st.out_bits & (1< 0) { - if (st.out_bits & (1< 0) { - if (st.out_bits & (1< 0) { - if (st.out_bits & (1<flag & SEGMENT_END_OF_BLOCK) { @@ -412,7 +361,6 @@ ISR(TIMER2_COMPA_vect) // Discard current segment by advancing buffer tail index if ( ++segment_buffer_tail == SEGMENT_BUFFER_SIZE) { segment_buffer_tail = 0; } - } st.out_bits ^= settings.invert_mask; // Apply step port invert mask @@ -486,6 +434,7 @@ void st_cycle_start() { if (sys.state == STATE_QUEUED) { sys.state = STATE_CYCLE; + st_prep_buffer(); // Initialize step segment buffer before beginning cycle. st_wake_up(); } } @@ -520,8 +469,8 @@ void st_cycle_reinitialize() // plan_cycle_reinitialize(st_current_data->step_events_remaining); // st.ramp_type = RAMP_ACCEL; -// st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; -// st.delta_d = 0; +// st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK/2; +// st.ramp_rate = 0; // sys.state = STATE_QUEUED; // } else { // sys.state = STATE_IDLE; @@ -580,6 +529,7 @@ void st_cycle_reinitialize() */ void st_prep_buffer() { + if (sys.state != STATE_QUEUED) { // Block until a motion state is issued while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer. // Initialize new segment @@ -605,7 +555,7 @@ void st_prep_buffer() st_prep_data->step_events_remaining = last_st_prep_data->step_events_remaining; st_prep_data->rate_delta = last_st_prep_data->rate_delta; - st_prep_data->d_next = last_st_prep_data->d_next; + st_prep_data->dist_next_step = last_st_prep_data->dist_next_step; st_prep_data->nominal_rate = last_st_prep_data->nominal_rate; // TODO: Feedrate overrides recomputes this. st_prep_data->mm_per_step = last_st_prep_data->mm_per_step; @@ -625,7 +575,7 @@ void st_prep_buffer() st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) st_prep_data->rate_delta = ceil(pl_prep_block->acceleration* ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) - st_prep_data->d_next = ceil((pl_prep_block->millimeters*INV_TIME_MULTIPLIER)/pl_prep_block->step_event_count); // (mult*mm/step) + st_prep_data->dist_next_step = ceil((pl_prep_block->millimeters*INV_TIME_MULTIPLIER)/pl_prep_block->step_event_count); // (mult*mm/step) // TODO: Check if we really need to store this. st_prep_data->mm_per_step = pl_prep_block->millimeters/pl_prep_block->step_event_count; @@ -656,7 +606,7 @@ void st_prep_buffer() // We do this to minimize memory and computational requirements. However, this could easily be replaced with // a more exact approximation or have a unique time per segment, if CPU and memory overhead allows. if (st_prep_data->decelerate_after <= 0) { - if (st_prep_data->decelerate_after == 0) { prep_segment->flag = SEGMENT_DECEL; } + if (st_prep_data->decelerate_after == 0) { prep_segment->flag = SEGMENT_DECEL; } // Set segment deceleration flag else { st_prep_data->current_approx_rate -= st_prep_data->rate_delta; } if (st_prep_data->current_approx_rate < st_prep_data->rate_delta) { st_prep_data->current_approx_rate >>= 1; } } else { @@ -669,9 +619,9 @@ void st_prep_buffer() } // Compute the number of steps in the prepped segment based on the approximate current rate. - // NOTE: The d_next divide cancels out the INV_TIME_MULTIPLIER and converts the rate value to steps. + // NOTE: The dist_next_step divide cancels out the INV_TIME_MULTIPLIER and converts the rate value to steps. prep_segment->n_step = ceil(max(MINIMUM_STEP_RATE,st_prep_data->current_approx_rate)* - (ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND)/st_prep_data->d_next); + (ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND)/st_prep_data->dist_next_step); // NOTE: Ensures it moves for very slow motions, but MINIMUM_STEP_RATE should always set this too. Perhaps // a compile-time check to see if MINIMUM_STEP_RATE is set high enough is all that is needed. prep_segment->n_step = max(prep_segment->n_step,MINIMUM_STEPS_PER_SEGMENT); @@ -697,6 +647,11 @@ void st_prep_buffer() // Check for end of planner block if ( st_prep_data->step_events_remaining == 0 ) { + + // TODO: When a feed hold ends, the step_events_remaining will also be zero, even though a block + // have partially been completed. We need to flag the stepper algorithm to indicate a stepper shutdown + // when complete, but not remove the planner block unless it truly is the end of the block (rare). + // Set EOB bitflag so stepper algorithm discards the planner block after this segment completes. prep_segment->flag |= SEGMENT_END_OF_BLOCK; // Move planner pointer to next block and flag to load a new block for the next segment. @@ -713,6 +668,7 @@ void st_prep_buffer() // printString(" "); } + } } uint8_t st_get_prep_block_index() diff --git a/stepper_old.c b/stepper_old.c index 3191007..d702eba 100644 --- a/stepper_old.c +++ b/stepper_old.c @@ -19,20 +19,32 @@ along with Grbl. If not, see . */ -/* The timer calculations of this module informed by the 'RepRap cartesian firmware' by Zack Smith - and Philipp Tiefenbacher. */ - #include #include "stepper.h" #include "config.h" #include "settings.h" #include "planner.h" +#include "nuts_bolts.h" // Some useful constants #define TICKS_PER_MICROSECOND (F_CPU/1000000) -#define CRUISE_RAMP 0 -#define ACCEL_RAMP 1 -#define DECEL_RAMP 2 + +#define RAMP_NOOP_CRUISE 0 +#define RAMP_ACCEL 1 +#define RAMP_DECEL 2 + +#define LOAD_NOOP 0 +#define LOAD_SEGMENT 1 +#define LOAD_BLOCK 2 + +#define SEGMENT_NOOP 0 +#define SEGMENT_END_OF_BLOCK bit(0) +#define SEGMENT_ACCEL bit(1) +#define SEGMENT_DECEL bit(2) + +#define MINIMUM_STEPS_PER_SEGMENT 1 // Don't change + +#define SEGMENT_BUFFER_SIZE 6 // Stepper state variable. Contains running data and trapezoid variables. typedef struct { @@ -40,44 +52,85 @@ typedef struct { int32_t counter_x, // Counter variables for the bresenham line tracer counter_y, counter_z; - int32_t event_count; // Total event count. Retained for feed holds. - int32_t step_events_remaining; // Steps remaining in motion + uint8_t segment_steps_remaining; // Steps remaining in line segment motion - // Used by Pramod Ranade inverse time algorithm - int32_t delta_d; // Ranade distance traveled per interrupt tick - int32_t d_counter; // Ranade distance traveled since last step event - uint8_t ramp_count; // Acceleration interrupt tick counter. - uint8_t ramp_type; // Ramp type variable. - uint8_t execute_step; // Flags step execution for each interrupt. + // Used by inverse time algorithm to track step rate + int32_t counter_d; // Inverse time distance traveled since last step event + uint32_t delta_d; // Inverse time distance traveled per interrupt tick + uint32_t d_per_tick; + // Used by the stepper driver interrupt + uint8_t execute_step; // Flags step execution for each interrupt. + uint8_t step_pulse_time; // Step pulse reset time after step rise + uint8_t out_bits; // The next stepping-bits to be output + uint8_t load_flag; + + uint8_t ramp_count; + uint8_t ramp_type; } stepper_t; static stepper_t st; -static block_t *current_block; // A pointer to the block currently being traced -// Used by the stepper driver interrupt -static uint8_t step_pulse_time; // Step pulse reset time after step rise -static uint8_t out_bits; // The next stepping-bits to be output +// Stores stepper common data for executing steps in the segment buffer. Data can change mid-block when the +// planner updates the remaining block velocity profile with a more optimal plan or a feedrate override occurs. +// NOTE: Normally, this buffer is partially in-use, but, for the worst case scenario, it will never exceed +// the number of accessible stepper buffer segments (SEGMENT_BUFFER_SIZE-1). +typedef struct { + int32_t step_events_remaining; // Tracks step event count for the executing planner block + uint32_t d_next; // Scaled distance to next step + uint32_t initial_rate; // Initialized step rate at re/start of a planner block + uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute + uint32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive) + uint32_t current_approx_rate; // Tracks the approximate segment rate to predict steps per segment to execute + int32_t decelerate_after; // Tracks when to initiate deceleration according to the planner block + float mm_per_step; +} st_data_t; +static st_data_t segment_data[SEGMENT_BUFFER_SIZE-1]; -// NOTE: If the main interrupt is guaranteed to be complete before the next interrupt, then -// this blocking variable is no longer needed. Only here for safety reasons. -static volatile uint8_t busy; // True when "Stepper Driver Interrupt" is being serviced. Used to avoid retriggering that handler. +// Primary stepper segment ring buffer. Contains small, short line segments for the stepper algorithm to execute, +// which are "checked-out" incrementally from the first block in the planner buffer. Once "checked-out", the steps +// in the segments buffer cannot be modified by the planner, where the remaining planner block steps still can. +typedef struct { + uint8_t n_step; // Number of step events to be executed for this segment + uint8_t st_data_index; // Stepper buffer common data index. Uses this information to execute this segment. + uint8_t flag; // Stepper algorithm bit-flag for special execution conditions. +} st_segment_t; +static st_segment_t segment_buffer[SEGMENT_BUFFER_SIZE]; -// __________________________ -// /| |\ _________________ ^ -// / | | \ /| |\ | -// / | | \ / | | \ s -// / | | | | | \ p -// / | | | | | \ e -// +-----+------------------------+---+--+---------------+----+ e -// | BLOCK 1 | BLOCK 2 | d -// -// time -----> -// -// The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta -// until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after -// after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as -// +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. +// Step segment ring buffer indices +static volatile uint8_t segment_buffer_tail; +static volatile uint8_t segment_buffer_head; +static uint8_t segment_next_head; +static volatile uint8_t busy; // Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though. +static plan_block_t *pl_current_block; // A pointer to the planner block currently being traced +static st_segment_t *st_current_segment; +static st_data_t *st_current_data; + +// Pointers for the step segment being prepped from the planner buffer. Accessed only by the +// main program. Pointers may be planning segments or planner blocks ahead of what being executed. +static plan_block_t *pl_prep_block; // Pointer to the planner block being prepped +static st_data_t *st_prep_data; // Pointer to the stepper common data being prepped +static uint8_t pl_prep_index; // Index of planner block being prepped +static uint8_t st_data_prep_index; // Index of stepper common data block being prepped +static uint8_t pl_partial_block_flag; // Flag indicating the planner has modified the prepped planner block + + +/* __________________________ + /| |\ _________________ ^ + / | | \ /| |\ | + / | | \ / | | \ s + / | | | | | \ p + / | | | | | \ e + +-----+------------------------+---+--+---------------+----+ e + | BLOCK 1 | BLOCK 2 | d + + time -----> + + The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta + until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after + after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as + +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. +*/ // Stepper state initialization. Cycle should only start if the st.cycle_start flag is // enabled. Startup init and limits call this function but shouldn't start the cycle. @@ -91,14 +144,17 @@ void st_wake_up() } if (sys.state == STATE_CYCLE) { // Initialize stepper output bits - out_bits = settings.invert_mask; + st.out_bits = settings.invert_mask; // Initialize step pulse timing from settings. - step_pulse_time = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3); + st.step_pulse_time = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3); // Enable stepper driver interrupt st.execute_step = false; + st.load_flag = LOAD_BLOCK; + TCNT2 = 0; // Clear Timer2 TIMSK2 |= (1<direction_bits ^ settings.invert_mask; - st.execute_step = true; // Set flag to set direction bits. + // NOTE: Loads after a step event. At high rates above 1/2 ISR frequency, there is + // a small chance that this will load at the same time as a step event. Hopefully, + // the overhead for this loading event isn't too much.. possibly 2-5 usec. + + // NOTE: The stepper algorithm must control the planner buffer tail as it completes + // the block moves. Otherwise, a feed hold can leave a few step buffer line moves + // without the correct planner block information. - // Initialize Bresenham variables - st.counter_x = (current_block->step_event_count >> 1); - st.counter_y = st.counter_x; - st.counter_z = st.counter_x; - st.event_count = current_block->step_event_count; - st.step_events_remaining = st.event_count; + st_current_segment = &segment_buffer[segment_buffer_tail]; - // During feed hold, do not update Ranade counter, rate, or ramp type. Keep decelerating. - if (sys.state == STATE_CYCLE) { - // Initialize Ranade variables - st.d_counter = current_block->d_next; - st.delta_d = current_block->initial_rate; - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; + // Load number of steps to execute from stepper buffer + st.segment_steps_remaining = st_current_segment->n_step; + + // Check if the counters need to be reset for a new planner block + if (st.load_flag == LOAD_BLOCK) { + pl_current_block = plan_get_current_block(); // Should always be there. Stepper buffer handles this. + st_current_data = &segment_data[segment_buffer[segment_buffer_tail].st_data_index]; //st_current_segment->st_data_index]; - // Initialize ramp type. - if (st.step_events_remaining == current_block->decelerate_after) { st.ramp_type = DECEL_RAMP; } - else if (st.delta_d == current_block->nominal_rate) { st.ramp_type = CRUISE_RAMP; } - else { st.ramp_type = ACCEL_RAMP; } + // Initialize direction bits for block + st.out_bits = pl_current_block->direction_bits ^ settings.invert_mask; + st.execute_step = true; // Set flag to set direction bits upon next ISR tick. + + // Initialize Bresenham line counters + st.counter_x = (pl_current_block->step_event_count >> 1); + st.counter_y = st.counter_x; + st.counter_z = st.counter_x; + + // Initialize inverse time and step rate counter data + st.counter_d = st_current_data->d_next; // d_next always greater than delta_d. + if (st.delta_d < MINIMUM_STEP_RATE) { st.d_per_tick = MINIMUM_STEP_RATE; } + else { st.d_per_tick = st.delta_d; } + + // During feed hold, do not update rate, ramp type, or ramp counters. Keep decelerating. +// if (sys.state == STATE_CYCLE) { + st.delta_d = st_current_data->initial_rate; + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Initialize ramp counter via midpoint rule + st.ramp_type = RAMP_NOOP_CRUISE; // Initialize as no ramp operation. Corrected later if necessary. +// } + + } + + // Acceleration and cruise handled by ramping. Just check if deceleration needs to begin. + if ( st_current_segment->flag & (SEGMENT_DECEL | SEGMENT_ACCEL) ) { + /* Compute correct ramp count for a ramp change. Upon a switch from acceleration to deceleration, + or vice-versa, the new ramp count must be set to trigger the next acceleration tick equal to + the number of ramp ISR ticks counted since the last acceleration tick. This is ensures the + ramp is executed exactly as the plan dictates. Otherwise, when a ramp begins from a known + rate (nominal/cruise or initial), the ramp count must be set to ISR_TICKS_PER_ACCELERATION_TICK/2 + as mandated by the mid-point rule. For these conditions, the ramp count have been initialized + such that the following computation is still correct. */ + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; + if ( st_current_segment->flag & SEGMENT_DECEL ) { st.ramp_type = RAMP_DECEL; } + else { st.ramp_type = RAMP_ACCEL; } } + st.load_flag = LOAD_NOOP; // Segment motion loaded. Set no-operation flag to skip during execution. + } else { + // Can't discard planner block here if a feed hold stops in middle of block. st_go_idle(); bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end return; // Nothing to do but exit. } + } // Adjust inverse time counter for ac/de-celerations - if (st.ramp_type) { - // Tick acceleration ramp counter - st.ramp_count--; - if (st.ramp_count == 0) { - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter - if (st.ramp_type == ACCEL_RAMP) { // Adjust velocity for acceleration - st.delta_d += current_block->rate_delta; - if (st.delta_d >= current_block->nominal_rate) { // Reached cruise state. - st.ramp_type = CRUISE_RAMP; - st.delta_d = current_block->nominal_rate; // Set cruise velocity + // NOTE: Accelerations are handled by the stepper algorithm as it's thought to be more computationally + // efficient on the Arduino AVR. This could may not be true with higher ISR frequencies or faster CPUs. + if (st.ramp_type) { // Ignored when ramp type is NOOP_CRUISE + st.ramp_count--; // Tick acceleration ramp counter + if (st.ramp_count == 0) { // Adjust step rate when its time + if (st.ramp_type == RAMP_ACCEL) { // Adjust velocity for acceleration + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter + st.delta_d += st_current_data->rate_delta; + if (st.delta_d >= st_current_data->nominal_rate) { // Reached nominal rate. + st.delta_d = st_current_data->nominal_rate; // Set cruising velocity + st.ramp_type = RAMP_NOOP_CRUISE; // Set ramp flag to cruising + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Re-initialize counter for next ramp. } - } else if (st.ramp_type == DECEL_RAMP) { // Adjust velocity for deceleration - if (st.delta_d > current_block->rate_delta) { - st.delta_d -= current_block->rate_delta; - } else { + } else { // Adjust velocity for deceleration. + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter + if (st.delta_d > st_current_data->rate_delta) { + st.delta_d -= st_current_data->rate_delta; + } else { // Moving near zero feed rate. Gracefully slow down. st.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow. + + // TODO: Check for and handle feed hold exit? At this point, machine is stopped. + // - Set system flag to recompute plan and reset segment buffer. + // - Segment steps in buffer needs to be returned to planner correctly. + // busy = false; + // return; + } } + // Finalize adjusted step rate. Ensure minimum. + if (st.delta_d < MINIMUM_STEP_RATE) { st.d_per_tick = MINIMUM_STEP_RATE; } + else { st.d_per_tick = st.delta_d; } } } - - // Iterate Pramod Ranade inverse time counter. Triggers each Bresenham step event. - if (st.delta_d < MINIMUM_STEP_RATE) { st.d_counter -= MINIMUM_STEP_RATE; } - else { st.d_counter -= st.delta_d; } + + // Iterate inverse time counter. Triggers each Bresenham step event. + st.counter_d -= st.d_per_tick; // Execute Bresenham step event, when it's time to do so. - if (st.d_counter < 0) { - st.d_counter += current_block->d_next; - - // Check for feed hold state and execute accordingly. - if (sys.state == STATE_HOLD) { - if (st.ramp_type != DECEL_RAMP) { - st.ramp_type = DECEL_RAMP; - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; - } - if (st.delta_d <= current_block->rate_delta) { - st_go_idle(); - bit_true(sys.execute,EXEC_CYCLE_STOP); - return; - } - } - - // TODO: Vary Bresenham resolution for smoother motions or enable faster step rates (>20kHz). - - out_bits = current_block->direction_bits; // Reset out_bits and reload direction bits + if (st.counter_d < 0) { + st.counter_d += st_current_data->d_next; // Reload inverse time counter + + st.out_bits = pl_current_block->direction_bits; // Reset out_bits and reload direction bits st.execute_step = true; // Execute step displacement profile by Bresenham line algorithm - st.counter_x -= current_block->steps[X_AXIS]; + st.counter_x -= pl_current_block->steps[X_AXIS]; if (st.counter_x < 0) { - out_bits |= (1<step_event_count; + // st.steps_x++; + if (st.out_bits & (1<steps[Y_AXIS]; + st.counter_y -= pl_current_block->steps[Y_AXIS]; if (st.counter_y < 0) { - out_bits |= (1<step_event_count; + // st.steps_y++; + if (st.out_bits & (1<steps[Z_AXIS]; + st.counter_z -= pl_current_block->steps[Z_AXIS]; if (st.counter_z < 0) { - out_bits |= (1<step_event_count; + // st.steps_z++; + if (st.out_bits & (1<decelerate_after) { - st.ramp_type = DECEL_RAMP; - if (st.step_events_remaining == current_block->decelerate_after) { - if (st.delta_d == current_block->nominal_rate) { - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid - } else { - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; // Set ramp counter for triangle - } - } - } + st.segment_steps_remaining--; // Decrement step events count + if (st.segment_steps_remaining == 0) { + /* + NOTE: sys.position updates could be done here. The bresenham counters can have + their own fast 8-bit addition-only counters. Here we would check the direction and + apply it to sys.position accordingly. However, this could take too much time + combined with loading a new segment during next cycle too. + TODO: Measure the time it would take in the worst case. It could still be faster + overall during segment execution if uint8 step counters tracked this and was added + to the system position variables here. Compared to worst case now, it wouldn't be + that much different. + + // TODO: Upon loading, step counters would need to be zeroed. + // TODO: For feedrate overrides, we will have to execute add these values.. although + // for probing, this breaks. Current values won't be correct, unless we query it. + // It makes things more complicated, but still manageable. + if (st.steps_x > 0) { + if (st.out_bits & (1< 0) { + if (st.out_bits & (1< 0) { + if (st.out_bits & (1<flag & SEGMENT_END_OF_BLOCK) { + plan_discard_current_block(); + st.load_flag = LOAD_BLOCK; + } else { + st.load_flag = LOAD_SEGMENT; } - } else { - // If current block is finished, reset pointer - current_block = NULL; - plan_discard_current_block(); + + // Discard current segment by advancing buffer tail index + if ( ++segment_buffer_tail == SEGMENT_BUFFER_SIZE) { segment_buffer_tail = 0; } + } - out_bits ^= settings.invert_mask; // Apply step port invert mask + st.out_bits ^= settings.invert_mask; // Apply step port invert mask } busy = false; // SPINDLE_ENABLE_PORT ^= 1<step_events_remaining); +// st.ramp_type = RAMP_ACCEL; +// st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; +// st.delta_d = 0; +// sys.state = STATE_QUEUED; +// } else { +// sys.state = STATE_IDLE; +// } sys.state = STATE_IDLE; - } + +} + + +/* Prepares step segment buffer. Continuously called from main program. + + The segment buffer is an intermediary buffer interface between the execution of steps + by the stepper algorithm and the velocity profiles generated by the planner. The stepper + algorithm only executes steps within the segment buffer and is filled by the main program + when steps are "checked-out" from the first block in the planner buffer. This keeps the + step execution and planning optimization processes atomic and protected from each other. + The number of steps "checked-out" from the planner buffer and the number of segments in + the segment buffer is sized and computed such that no operation in the main program takes + longer than the time it takes the stepper algorithm to empty it before refilling it. + Currently, the segment buffer conservatively holds roughly up to 40-60 msec of steps. + + NOTE: The segment buffer executes a set number of steps over an approximate time period. + If we try to execute over a set time period, it is difficult to guarantee or predict how + many steps will execute over it, especially when the step pulse phasing between the + neighboring segments are kept consistent. Meaning that, if the last segment step pulses + right before its end, the next segment must delay its first pulse so that the step pulses + are consistently spaced apart over time to keep the step pulse train nice and smooth. + Keeping track of phasing and ensuring that the exact number of steps are executed as + defined by the planner block, the related computational overhead gets quickly and + prohibitively expensive, especially in real-time. + Since the stepper algorithm automatically takes care of the step pulse phasing with + its ramp and inverse time counters, we don't have to explicitly and expensively track the + exact number of steps, time, or phasing of steps. All we need to do is approximate + the number of steps in each segment such that the segment buffer has enough execution time + for the main program to do what it needs to do and refill it when it has time. In other + words, we just need to compute a cheap approximation of the current velocity and the + number of steps over it. +*/ + +/* + TODO: Figure out how to enforce a deceleration when a feedrate override is reduced. + The problem is that when an override is reduced, the planner may not plan back to + the current rate. Meaning that the velocity profiles for certain conditions no longer + are trapezoidal or triangular. For example, if the current block is cruising at a + nominal rate and the feedrate override is reduced, the new nominal rate will now be + lower. The velocity profile must first decelerate to the new nominal rate and then + follow on the new plan. So the remaining velocity profile will have a decelerate, + cruise, and another decelerate. + Another issue is whether or not a feedrate override reduction causes a deceleration + that acts over several planner blocks. For example, say that the plan is already + heavily decelerating throughout it, reducing the feedrate will not do much to it. So, + how do we determine when to resume the new plan? How many blocks do we have to wait + until the new plan intersects with the deceleration curve? One plus though, the + deceleration will never be more than the number of blocks in the entire planner buffer, + but it theoretically can be equal to it when all planner blocks are decelerating already. +*/ +void st_prep_buffer() +{ + while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer. + + // Initialize new segment + st_segment_t *prep_segment = &segment_buffer[segment_buffer_head]; + prep_segment->flag = SEGMENT_NOOP; + + // Determine if we need to load a new planner block. + if (pl_prep_block == NULL) { + pl_prep_block = plan_get_block_by_index(pl_prep_index); // Query planner for a queued block + if (pl_prep_block == NULL) { return; } // No planner blocks. Exit. + + // Increment stepper common data index + if ( ++st_data_prep_index == (SEGMENT_BUFFER_SIZE-1) ) { st_data_prep_index = 0; } + + // Check if the planner has re-computed this block mid-execution. If so, push the previous segment + // data. Otherwise, prepare a new segment data for the new planner block. + if (pl_partial_block_flag) { + + // Prepare new shared segment block data and copy the relevant last segment block data. + st_data_t *last_st_prep_data; + last_st_prep_data = st_prep_data; + st_prep_data = &segment_data[st_data_prep_index]; + + st_prep_data->step_events_remaining = last_st_prep_data->step_events_remaining; + st_prep_data->rate_delta = last_st_prep_data->rate_delta; + st_prep_data->d_next = last_st_prep_data->d_next; + st_prep_data->nominal_rate = last_st_prep_data->nominal_rate; // TODO: Feedrate overrides recomputes this. + + st_prep_data->mm_per_step = last_st_prep_data->mm_per_step; + + pl_partial_block_flag = false; // Reset flag + + } else { + + // Prepare commonly shared planner block data for the ensuing segment buffer moves ad-hoc, since + // the planner buffer can dynamically change the velocity profile data as blocks are added. + st_prep_data = &segment_data[st_data_prep_index]; + + // Initialize Bresenham variables + st_prep_data->step_events_remaining = pl_prep_block->step_event_count; + + // Convert planner block velocity profile data to stepper rate and step distance data. + st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + st_prep_data->rate_delta = ceil(pl_prep_block->acceleration* + ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) + st_prep_data->d_next = ceil((pl_prep_block->millimeters*INV_TIME_MULTIPLIER)/pl_prep_block->step_event_count); // (mult*mm/step) + + // TODO: Check if we really need to store this. + st_prep_data->mm_per_step = pl_prep_block->millimeters/pl_prep_block->step_event_count; + + } + + // Convert planner entry speed to stepper initial rate. + st_prep_data->initial_rate = ceil(sqrt(pl_prep_block->entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + + // TODO: Nominal rate changes with feedrate override. + // st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + + st_prep_data->current_approx_rate = st_prep_data->initial_rate; + + // Calculate the planner block velocity profile type, determine deceleration point, and initial ramp. + float mm_decelerate_after = plan_calculate_velocity_profile(pl_prep_index); + st_prep_data->decelerate_after = ceil( mm_decelerate_after/st_prep_data->mm_per_step ); + if (st_prep_data->decelerate_after > 0) { // If 0, SEGMENT_DECEL flag is set later. + if (st_prep_data->initial_rate != st_prep_data->nominal_rate) { prep_segment->flag = SEGMENT_ACCEL; } + } + } + + // Set new segment to point to the current segment data block. + prep_segment->st_data_index = st_data_prep_index; + + // Approximate the velocity over the new segment using the already computed rate values. + // NOTE: This assumes that each segment will have an execution time roughly equal to every ACCELERATION_TICK. + // We do this to minimize memory and computational requirements. However, this could easily be replaced with + // a more exact approximation or have a unique time per segment, if CPU and memory overhead allows. + if (st_prep_data->decelerate_after <= 0) { + if (st_prep_data->decelerate_after == 0) { prep_segment->flag = SEGMENT_DECEL; } // Set segment deceleration flag + else { st_prep_data->current_approx_rate -= st_prep_data->rate_delta; } + if (st_prep_data->current_approx_rate < st_prep_data->rate_delta) { st_prep_data->current_approx_rate >>= 1; } + } else { + if (st_prep_data->current_approx_rate < st_prep_data->nominal_rate) { + st_prep_data->current_approx_rate += st_prep_data->rate_delta; + if (st_prep_data->current_approx_rate > st_prep_data->nominal_rate) { + st_prep_data->current_approx_rate = st_prep_data->nominal_rate; + } + } + } + + // Compute the number of steps in the prepped segment based on the approximate current rate. + // NOTE: The d_next divide cancels out the INV_TIME_MULTIPLIER and converts the rate value to steps. + prep_segment->n_step = ceil(max(MINIMUM_STEP_RATE,st_prep_data->current_approx_rate)* + (ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND)/st_prep_data->d_next); + // NOTE: Ensures it moves for very slow motions, but MINIMUM_STEP_RATE should always set this too. Perhaps + // a compile-time check to see if MINIMUM_STEP_RATE is set high enough is all that is needed. + prep_segment->n_step = max(prep_segment->n_step,MINIMUM_STEPS_PER_SEGMENT); + // NOTE: As long as the ACCELERATION_TICKS_PER_SECOND is valid, n_step should never exceed 255 and overflow. + // prep_segment->n_step = min(prep_segment->n_step,MAXIMUM_STEPS_PER_BLOCK); // Prevent unsigned int8 overflow. + + // Check if n_step exceeds steps remaining in planner block. If so, truncate. + if (prep_segment->n_step > st_prep_data->step_events_remaining) { + prep_segment->n_step = st_prep_data->step_events_remaining; + } + + // Check if n_step crosses decelerate point in block. If so, truncate to ensure the deceleration + // ramp counters are set correctly during execution. + if (st_prep_data->decelerate_after > 0) { + if (prep_segment->n_step > st_prep_data->decelerate_after) { + prep_segment->n_step = st_prep_data->decelerate_after; + } + } + + // Update stepper common data variables. + st_prep_data->decelerate_after -= prep_segment->n_step; + st_prep_data->step_events_remaining -= prep_segment->n_step; + + // Check for end of planner block + if ( st_prep_data->step_events_remaining == 0 ) { + // Set EOB bitflag so stepper algorithm discards the planner block after this segment completes. + prep_segment->flag |= SEGMENT_END_OF_BLOCK; + // Move planner pointer to next block and flag to load a new block for the next segment. + pl_prep_index = plan_next_block_index(pl_prep_index); + pl_prep_block = NULL; + } + + // New step segment completed. Increment segment buffer indices. + segment_buffer_head = segment_next_head; + if ( ++segment_next_head == SEGMENT_BUFFER_SIZE ) { segment_next_head = 0; } + +// long a = prep_segment->n_step; +// printInteger(a); +// printString(" "); + + } +} + +uint8_t st_get_prep_block_index() +{ +// Returns only the index but doesn't state if the block has been partially executed. How do we simply check for this? + return(pl_prep_index); +} + +void st_fetch_partial_block_parameters(uint8_t block_index, float *millimeters_remaining, uint8_t *is_decelerating) +{ + // if called, can we assume that this always changes and needs to be updated? if so, then + // we can perform all of the segment buffer setup tasks here to make sure the next time + // the segments are loaded, the st_data buffer is updated correctly. + // !!! Make sure that this is always pointing to the correct st_prep_data block. + + // When a mid-block acceleration occurs, we have to make sure the ramp counters are updated + // correctly, much in the same fashion as the deceleration counters. Need to think about this + // make sure this is right, but i'm pretty sure it is. + + // TODO: NULL means that the segment buffer has just completed a planner block. Clean up! + if (pl_prep_block != NULL) { + *millimeters_remaining = st_prep_data->step_events_remaining*st_prep_data->mm_per_step; + if (st_prep_data->decelerate_after > 0) { *is_decelerating = false; } + else { *is_decelerating = true; } + + // Flag for new prep_block when st_prep_buffer() is called after the planner recomputes. + pl_partial_block_flag = true; + pl_prep_block = NULL; + } + return; } diff --git a/stepper_v0_9.c b/stepper_v0_9.c new file mode 100644 index 0000000..3191007 --- /dev/null +++ b/stepper_v0_9.c @@ -0,0 +1,387 @@ +/* + stepper.c - stepper motor driver: executes motion plans using stepper motors + Part of Grbl + + Copyright (c) 2011-2013 Sungeun K. Jeon + Copyright (c) 2009-2011 Simen Svale Skogsrud + + 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 + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +/* The timer calculations of this module informed by the 'RepRap cartesian firmware' by Zack Smith + and Philipp Tiefenbacher. */ + +#include +#include "stepper.h" +#include "config.h" +#include "settings.h" +#include "planner.h" + +// Some useful constants +#define TICKS_PER_MICROSECOND (F_CPU/1000000) +#define CRUISE_RAMP 0 +#define ACCEL_RAMP 1 +#define DECEL_RAMP 2 + +// Stepper state variable. Contains running data and trapezoid variables. +typedef struct { + // Used by the bresenham line algorithm + int32_t counter_x, // Counter variables for the bresenham line tracer + counter_y, + counter_z; + int32_t event_count; // Total event count. Retained for feed holds. + int32_t step_events_remaining; // Steps remaining in motion + + // Used by Pramod Ranade inverse time algorithm + int32_t delta_d; // Ranade distance traveled per interrupt tick + int32_t d_counter; // Ranade distance traveled since last step event + uint8_t ramp_count; // Acceleration interrupt tick counter. + uint8_t ramp_type; // Ramp type variable. + uint8_t execute_step; // Flags step execution for each interrupt. + +} stepper_t; +static stepper_t st; +static block_t *current_block; // A pointer to the block currently being traced + +// Used by the stepper driver interrupt +static uint8_t step_pulse_time; // Step pulse reset time after step rise +static uint8_t out_bits; // The next stepping-bits to be output + +// NOTE: If the main interrupt is guaranteed to be complete before the next interrupt, then +// this blocking variable is no longer needed. Only here for safety reasons. +static volatile uint8_t busy; // True when "Stepper Driver Interrupt" is being serviced. Used to avoid retriggering that handler. + +// __________________________ +// /| |\ _________________ ^ +// / | | \ /| |\ | +// / | | \ / | | \ s +// / | | | | | \ p +// / | | | | | \ e +// +-----+------------------------+---+--+---------------+----+ e +// | BLOCK 1 | BLOCK 2 | d +// +// time -----> +// +// The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta +// until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after +// after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as +// +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. + + +// Stepper state initialization. Cycle should only start if the st.cycle_start flag is +// enabled. Startup init and limits call this function but shouldn't start the cycle. +void st_wake_up() +{ + // Enable steppers by resetting the stepper disable port + if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { + STEPPERS_DISABLE_PORT |= (1<> 3); + // Enable stepper driver interrupt + st.execute_step = false; + TCNT2 = 0; // Clear Timer2 + TIMSK2 |= (1<direction_bits ^ settings.invert_mask; + st.execute_step = true; // Set flag to set direction bits. + + // Initialize Bresenham variables + st.counter_x = (current_block->step_event_count >> 1); + st.counter_y = st.counter_x; + st.counter_z = st.counter_x; + st.event_count = current_block->step_event_count; + st.step_events_remaining = st.event_count; + + // During feed hold, do not update Ranade counter, rate, or ramp type. Keep decelerating. + if (sys.state == STATE_CYCLE) { + // Initialize Ranade variables + st.d_counter = current_block->d_next; + st.delta_d = current_block->initial_rate; + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; + + // Initialize ramp type. + if (st.step_events_remaining == current_block->decelerate_after) { st.ramp_type = DECEL_RAMP; } + else if (st.delta_d == current_block->nominal_rate) { st.ramp_type = CRUISE_RAMP; } + else { st.ramp_type = ACCEL_RAMP; } + } + + } else { + st_go_idle(); + bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end + return; // Nothing to do but exit. + } + } + + // Adjust inverse time counter for ac/de-celerations + if (st.ramp_type) { + // Tick acceleration ramp counter + st.ramp_count--; + if (st.ramp_count == 0) { + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter + if (st.ramp_type == ACCEL_RAMP) { // Adjust velocity for acceleration + st.delta_d += current_block->rate_delta; + if (st.delta_d >= current_block->nominal_rate) { // Reached cruise state. + st.ramp_type = CRUISE_RAMP; + st.delta_d = current_block->nominal_rate; // Set cruise velocity + } + } else if (st.ramp_type == DECEL_RAMP) { // Adjust velocity for deceleration + if (st.delta_d > current_block->rate_delta) { + st.delta_d -= current_block->rate_delta; + } else { + st.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow. + } + } + } + } + + // Iterate Pramod Ranade inverse time counter. Triggers each Bresenham step event. + if (st.delta_d < MINIMUM_STEP_RATE) { st.d_counter -= MINIMUM_STEP_RATE; } + else { st.d_counter -= st.delta_d; } + + // Execute Bresenham step event, when it's time to do so. + if (st.d_counter < 0) { + st.d_counter += current_block->d_next; + + // Check for feed hold state and execute accordingly. + if (sys.state == STATE_HOLD) { + if (st.ramp_type != DECEL_RAMP) { + st.ramp_type = DECEL_RAMP; + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; + } + if (st.delta_d <= current_block->rate_delta) { + st_go_idle(); + bit_true(sys.execute,EXEC_CYCLE_STOP); + return; + } + } + + // TODO: Vary Bresenham resolution for smoother motions or enable faster step rates (>20kHz). + + out_bits = current_block->direction_bits; // Reset out_bits and reload direction bits + st.execute_step = true; + + // Execute step displacement profile by Bresenham line algorithm + st.counter_x -= current_block->steps[X_AXIS]; + if (st.counter_x < 0) { + out_bits |= (1<steps[Y_AXIS]; + if (st.counter_y < 0) { + out_bits |= (1<steps[Z_AXIS]; + if (st.counter_z < 0) { + out_bits |= (1<decelerate_after) { + st.ramp_type = DECEL_RAMP; + if (st.step_events_remaining == current_block->decelerate_after) { + if (st.delta_d == current_block->nominal_rate) { + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid + } else { + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; // Set ramp counter for triangle + } + } + } + } + } else { + // If current block is finished, reset pointer + current_block = NULL; + plan_discard_current_block(); + } + + out_bits ^= settings.invert_mask; // Apply step port invert mask + } + busy = false; +// SPINDLE_ENABLE_PORT ^= 1< Date: Tue, 29 Oct 2013 18:55:55 -0600 Subject: [PATCH 18/73] Planner function call fix. More clean up. --- planner.c | 172 +++++++++++++++++++++++++---------------------------- protocol.c | 2 + stepper.c | 61 +++++++++---------- 3 files changed, 115 insertions(+), 120 deletions(-) diff --git a/planner.c b/planner.c index c1d0350..0afa018 100644 --- a/planner.c +++ b/planner.c @@ -148,34 +148,18 @@ void plan_update_partial_block(uint8_t block_index, float exit_speed_sqr) recomputed as stated in the general guidelines. Planner buffer index mapping: - - block_buffer_head: Points to the newest incoming buffer block just added by plan_buffer_line(). The - planner never touches the exit speed of this block, which always defaults to 0. - block_buffer_tail: Points to the beginning of the planner buffer. First to be executed or being executed. - Can dynamically change with the old stepper algorithm, but with the new algorithm, this should be impossible - as long as the segment buffer is not empty. - - next_buffer_head: Points to next planner buffer block after the last block. Should always be empty. - - block_buffer_safe: Points to the first planner block in the buffer for which it is safe to change. Since - the stepper can be executing the first block and if the planner changes its conditions, this will cause - a discontinuity and error in the stepper profile with lost steps likely. With the new stepper algorithm, - the block_buffer_safe is always where the stepper segment buffer ends and can never be overwritten, but - this can change the state of the block profile from a pure trapezoid assumption. Meaning, if that block - is decelerating, the planner conditions can change such that the block can new accelerate mid-block. - - !!! I need to make sure that the stepper algorithm can modify the acceleration mid-block. Needed for feedrate overrides too. - - !!! planner_recalculate() may not work correctly with re-planning.... may need to artificially set both the - block_buffer_head and next_buffer_head back one index so that this works correctly, or allow the operation - of this function to accept two different conditions to operate on. - - - block_buffer_planned: Points to the first buffer block after the last optimally fixed block, which can no longer be - improved. This block and the trailing buffer blocks that can still be altered when new blocks are added. This planned - block points to the transition point between the fixed and non-fixed states and is handled slightly different. The entry - speed is fixed, indicating the reverse pass cannot maximize the speed further, but the velocity profile within it - can still be changed, meaning the forward pass calculations must start from here and influence the following block - entry speed. - - !!! Need to check if this is the start of the non-optimal or the end of the optimal block. - + - block_buffer_head: Points to the buffer block after the last block in the buffer. Used to indicate whether + the buffer is full or empty. As described for standard ring buffers, this block is always empty. + - next_buffer_head: Points to next planner buffer block after the buffer head block. When equal to the + buffer tail, this indicates the buffer is full. + - block_buffer_safe: Points to the first sequential planner block for which it is safe to recompute, which + is defined to be where the stepper's step segment buffer ends. This may or may not be the buffer tail, + since the step segment buffer queues steps which may have not finished executing and could span a few + blocks, if the block moves are very short. + - block_buffer_planned: Points to the first buffer block after the last optimally planned block for normal + streaming operating conditions. Use for planning optimizations by avoiding recomputing parts of the + planner buffer that don't change with the addition of a new block, as describe above. NOTE: All planner computations are performed in floating point to minimize numerical round-off errors. When a planner block is executed, the floating point values are converted to fast integers by the stepper @@ -198,7 +182,7 @@ static void planner_recalculate() { // Initialize block index to the last block in the planner buffer. - uint8_t block_index = prev_block_index(block_buffer_head); + uint8_t block_index = plan_prev_block_index(block_buffer_head); // Query stepper module for safe planner block index to recalculate to, which corresponds to the end // of the step segment buffer. @@ -324,58 +308,6 @@ static void planner_recalculate() } -/* - uint8_t block_buffer_safe = st_get_prep_block_index(); - if (block_buffer_head == block_buffer_safe) { // Also catches head = tail - block_buffer_planned = block_buffer_head; - } else { - uint8_t block_index = block_buffer_head; - plan_block_t *current = &block_buffer[block_index]; - current->entry_speed_sqr = min( current->max_entry_speed_sqr, 2*current->acceleration*current->millimeters); - float entry_speed_sqr; - plan_block_t *next; - block_index = plan_prev_block_index(block_index); - if (block_index == block_buffer_safe) { // !! OR plan pointer? Yes I think so. - plan_update_partial_block(block_index, 0.0); - block_buffer_planned = block_index; - } else { - while (block_index != block_buffer_planned) { - next = current; - current = &block_buffer[block_index]; - block_index = plan_prev_block_index(block_index); - if (block_index == block_buffer_safe) { - plan_update_partial_block(block_index,current->entry_speed_sqr); - block_buffer_planned = block_index; - } - if (current->entry_speed_sqr != current->max_entry_speed_sqr) { - entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters; - if (entry_speed_sqr < current->max_entry_speed_sqr) { - current->entry_speed_sqr = entry_speed_sqr; - } else { - current->entry_speed_sqr = current->max_entry_speed_sqr; - } - } - } - - } - next = &block_buffer[block_buffer_planned]; // Begin at buffer planned pointer - block_index = plan_next_block_index(block_buffer_planned); - while (block_index != next_buffer_head) { - current = next; - next = &block_buffer[block_index]; - if (current->entry_speed_sqr < next->entry_speed_sqr) { - entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters; - if (entry_speed_sqr < next->entry_speed_sqr) { - next->entry_speed_sqr = entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass sets this. - block_buffer_planned = block_index; // Set optimal plan pointer. - } - } - if (next->entry_speed_sqr == next->max_entry_speed_sqr) { - block_buffer_planned = block_index; // Set optimal plan pointer - } - block_index = plan_next_block_index( block_index ); - } - } */ } @@ -439,16 +371,16 @@ void plan_synchronize() } -// Add a new linear movement to the buffer. target[N_AXIS] is the signed, absolute target position -// in millimeters. 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. -// All position data passed to the planner must be in terms of machine position to keep the planner -// independent of any coordinate system changes and offsets, which are handled by the g-code parser. -// NOTE: Assumes buffer is available. Buffer checks are handled at a higher level by motion_control. -// In other words, the buffer head is never equal to the buffer tail. Also the feed rate input value -// is used in three ways: as a normal feed rate if invert_feed_rate is false, as inverse time if -// invert_feed_rate is true, or as seek/rapids rate if the feed_rate value is negative (and -// invert_feed_rate always false). +/* Add a new linear movement to the buffer. target[N_AXIS] is the signed, absolute target position + in millimeters. 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. + All position data passed to the planner must be in terms of machine position to keep the planner + independent of any coordinate system changes and offsets, which are handled by the g-code parser. + NOTE: Assumes buffer is available. Buffer checks are handled at a higher level by motion_control. + In other words, the buffer head is never equal to the buffer tail. Also the feed rate input value + is used in three ways: as a normal feed rate if invert_feed_rate is false, as inverse time if + invert_feed_rate is true, or as seek/rapids rate if the feed_rate value is negative (and + invert_feed_rate always false). */ void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) { // Prepare and initialize new block @@ -675,3 +607,63 @@ void plan_cycle_reinitialize(int32_t step_events_remaining) block_buffer_planned = block_buffer_tail; planner_recalculate(); } + + +/* +TODO: + When a feed hold or feedrate override is reduced, the velocity profile must execute a + deceleration over the existing plan. By logic, since the plan already decelerates to zero + at the end of the buffer, any replanned deceleration mid-way will never exceed this. It + will only asymptotically approach this in the worst case scenario. + + - For a feed hold, we simply need to plan and compute the stopping point within a block + when velocity decelerates to zero. We then can recompute the plan with the already + existing partial block planning code and set the system to a QUEUED state. + - When a feed hold is initiated, the main program should be able to continue doing what + it has been, i.e. arcs, parsing, but needs to be able to reinitialize the plan after + it has come to a stop. + + - For a feed rate override (reduce-only), we need to enforce a deceleration until we + intersect the reduced nominal speed of a block after it's been planned with the new + overrides and the newly planned block is accelerating or cruising only. If the new plan + block is decelerating at the intersection point, we keep decelerating until we find a + valid intersection point. Once we find this point, we can then resume onto the new plan, + but we may need to adjust the deceleration point in the intersection block since the + feedrate override could have intersected at an acceleration ramp. This would change the + acceleration ramp to a cruising, so the deceleration point will have changed, but the + plan will have not. It should still be valid for the rest of the buffer. Coding this + can get complicated, but it should be doable. One issue could be is in how to handle + scenarios when a user issues several feedrate overrides and inundates this code. Does + this method still work and is robust enough to compute all of this on the fly? This is + the critical question. However, we could block user input until the planner has time to + catch to solve this as well. + + - When the feed rate override increases, we don't have to do anything special. We just + replan the entire buffer with the new nominal speeds and adjust the maximum junction + speeds accordingly. + +void plan_compute_deceleration() { + +} + + +void plan_recompute_max_junction_velocity() { + // Assumes the nominal_speed_sqr values have been updated. May need to just multiply + // override values here. + // PROBLEM: Axes-limiting velocities get screwed up. May need to store an int8 value for the + // max override value possible for each block when the line is added. So the nominal_speed + // is computed with that ceiling, but still retained if the rates change again. + uint8_t block_index = block_buffer_tail; + plan_block_t *block = &block_buffer[block_index]; + pl.previous_nominal_speed_sqr = block->nominal_speed_sqr; + block_index = plan_next_block_index(block_index); + while (block_index != block_buffer_head) { + block = &block_buffer[block_index]; + block->max_entry_speed_sqr = min(block->max_junction_speed_sqr, + min(block->nominal_speed_sqr,pl.previous_nominal_speed_sqr)); + pl.previous_nominal_speed_sqr = block->nominal_speed_sqr; + block_index = plan_next_block_index(block_index); + } +} + +*/ diff --git a/protocol.c b/protocol.c index 5466720..014b607 100644 --- a/protocol.c +++ b/protocol.c @@ -153,6 +153,8 @@ void protocol_execute_runtime() // Initiate stepper feed hold if (rt_exec & EXEC_FEED_HOLD) { + // !!! During a cycle, the segment buffer has just been reloaded and full. So the math involved + // with the feed hold should be fine for most, if not all, operational scenarios. st_feed_hold(); // Initiate feed hold. bit_false(sys.execute,EXEC_FEED_HOLD); } diff --git a/stepper.c b/stepper.c index 22f1edd..e3e4b25 100644 --- a/stepper.c +++ b/stepper.c @@ -39,8 +39,8 @@ #define SEGMENT_NOOP 0 #define SEGMENT_END_OF_BLOCK bit(0) -#define SEGMENT_ACCEL bit(1) -#define SEGMENT_DECEL bit(2) +#define RAMP_CHANGE_ACCEL bit(1) +#define RAMP_CHANGE_DECEL bit(2) #define MINIMUM_STEPS_PER_SEGMENT 1 // Don't change @@ -76,7 +76,7 @@ static stepper_t st; // the number of accessible stepper buffer segments (SEGMENT_BUFFER_SIZE-1). typedef struct { int32_t step_events_remaining; // Tracks step event count for the executing planner block - uint32_t dist_next_step; // Scaled distance to next step + uint32_t dist_per_step; // Scaled distance to next step uint32_t initial_rate; // Initialized step rate at re/start of a planner block uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute uint32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive) @@ -254,7 +254,7 @@ ISR(TIMER2_COMPA_vect) st.counter_z = st.counter_x; // Initialize inverse time, step rate data, and acceleration ramp counters - st.counter_dist = st_current_data->dist_next_step; // dist_next_step always greater than ramp_rate. + st.counter_dist = st_current_data->dist_per_step; // dist_per_step always greater than ramp_rate. st.ramp_rate = st_current_data->initial_rate; st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK/2; // Initialize ramp counter via midpoint rule st.ramp_type = RAMP_NOOP_CRUISE; // Initialize as no ramp operation. Corrected later if necessary. @@ -265,7 +265,7 @@ ISR(TIMER2_COMPA_vect) } // Check if ramp conditions have changed. If so, update ramp counters and control variables. - if ( st_current_segment->flag & (SEGMENT_DECEL | SEGMENT_ACCEL) ) { + if ( st_current_segment->flag & (RAMP_CHANGE_DECEL | RAMP_CHANGE_ACCEL) ) { /* Compute correct ramp count for a ramp change. Upon a switch from acceleration to deceleration, or vice-versa, the new ramp count must be set to trigger the next acceleration tick equal to the number of ramp ISR ticks counted since the last acceleration tick. This is ensures the @@ -274,7 +274,7 @@ ISR(TIMER2_COMPA_vect) as mandated by the mid-point rule. For the latter conditions, the ramp count have been initialized such that the following computation is still correct. */ st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK-st.counter_ramp; - if ( st_current_segment->flag & SEGMENT_DECEL ) { st.ramp_type = RAMP_DECEL; } + if ( st_current_segment->flag & RAMP_CHANGE_DECEL ) { st.ramp_type = RAMP_DECEL; } else { st.ramp_type = RAMP_ACCEL; } } @@ -319,7 +319,7 @@ ISR(TIMER2_COMPA_vect) // Execute Bresenham step event, when it's time to do so. if (st.counter_dist < 0) { - st.counter_dist += st_current_data->dist_next_step; // Reload inverse time counter + st.counter_dist += st_current_data->dist_per_step; // Reload inverse time counter st.out_bits = pl_current_block->direction_bits; // Reset out_bits and reload direction bits st.execute_step = true; @@ -493,21 +493,21 @@ void st_cycle_reinitialize() Currently, the segment buffer conservatively holds roughly up to 40-60 msec of steps. NOTE: The segment buffer executes a set number of steps over an approximate time period. - If we try to execute over a set time period, it is difficult to guarantee or predict how - many steps will execute over it, especially when the step pulse phasing between the - neighboring segments are kept consistent. Meaning that, if the last segment step pulses - right before its end, the next segment must delay its first pulse so that the step pulses - are consistently spaced apart over time to keep the step pulse train nice and smooth. - Keeping track of phasing and ensuring that the exact number of steps are executed as - defined by the planner block, the related computational overhead gets quickly and + If we try to execute over a fixed time period, it is difficult to guarantee or predict + how many steps will execute over it, especially when the step pulse phasing between the + neighboring segments must also be kept consistent. Meaning that, if the last segment step + pulses right before a segment end, the next segment must delay its first pulse so that the + step pulses are consistently spaced apart over time to keep the step pulse train nice and + smooth. Keeping track of phasing and ensuring that the exact number of steps are executed + as defined by the planner block, the related computational overhead can get quickly and prohibitively expensive, especially in real-time. Since the stepper algorithm automatically takes care of the step pulse phasing with - its ramp and inverse time counters, we don't have to explicitly and expensively track the - exact number of steps, time, or phasing of steps. All we need to do is approximate - the number of steps in each segment such that the segment buffer has enough execution time - for the main program to do what it needs to do and refill it when it has time. In other - words, we just need to compute a cheap approximation of the current velocity and the - number of steps over it. + its ramp and inverse time counters by retaining the count remainders, we don't have to + explicitly and expensively track and synchronize the exact number of steps, time, and + phasing of steps. All we need to do is approximate the number of steps in each segment + such that the segment buffer has enough execution time for the main program to do what + it needs to do and refill it when it comes back. In other words, we just need to compute + a cheap approximation of the current velocity and the number of steps over it. */ /* @@ -529,7 +529,7 @@ void st_cycle_reinitialize() */ void st_prep_buffer() { - if (sys.state != STATE_QUEUED) { // Block until a motion state is issued + if (sys.state == STATE_QUEUED) { return; } // Block until a motion state is issued while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer. // Initialize new segment @@ -555,7 +555,7 @@ void st_prep_buffer() st_prep_data->step_events_remaining = last_st_prep_data->step_events_remaining; st_prep_data->rate_delta = last_st_prep_data->rate_delta; - st_prep_data->dist_next_step = last_st_prep_data->dist_next_step; + st_prep_data->dist_per_step = last_st_prep_data->dist_per_step; st_prep_data->nominal_rate = last_st_prep_data->nominal_rate; // TODO: Feedrate overrides recomputes this. st_prep_data->mm_per_step = last_st_prep_data->mm_per_step; @@ -575,7 +575,7 @@ void st_prep_buffer() st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) st_prep_data->rate_delta = ceil(pl_prep_block->acceleration* ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) - st_prep_data->dist_next_step = ceil((pl_prep_block->millimeters*INV_TIME_MULTIPLIER)/pl_prep_block->step_event_count); // (mult*mm/step) + st_prep_data->dist_per_step = ceil((pl_prep_block->millimeters*INV_TIME_MULTIPLIER)/pl_prep_block->step_event_count); // (mult*mm/step) // TODO: Check if we really need to store this. st_prep_data->mm_per_step = pl_prep_block->millimeters/pl_prep_block->step_event_count; @@ -593,8 +593,8 @@ void st_prep_buffer() // Calculate the planner block velocity profile type, determine deceleration point, and initial ramp. float mm_decelerate_after = plan_calculate_velocity_profile(pl_prep_index); st_prep_data->decelerate_after = ceil( mm_decelerate_after/st_prep_data->mm_per_step ); - if (st_prep_data->decelerate_after > 0) { // If 0, SEGMENT_DECEL flag is set later. - if (st_prep_data->initial_rate != st_prep_data->nominal_rate) { prep_segment->flag = SEGMENT_ACCEL; } + if (st_prep_data->decelerate_after > 0) { // If 0, RAMP_CHANGE_DECEL flag is set later. + if (st_prep_data->initial_rate != st_prep_data->nominal_rate) { prep_segment->flag = RAMP_CHANGE_ACCEL; } } } @@ -604,9 +604,9 @@ void st_prep_buffer() // Approximate the velocity over the new segment using the already computed rate values. // NOTE: This assumes that each segment will have an execution time roughly equal to every ACCELERATION_TICK. // We do this to minimize memory and computational requirements. However, this could easily be replaced with - // a more exact approximation or have a unique time per segment, if CPU and memory overhead allows. + // a more exact approximation or have an user-defined time per segment, if CPU and memory overhead allows. if (st_prep_data->decelerate_after <= 0) { - if (st_prep_data->decelerate_after == 0) { prep_segment->flag = SEGMENT_DECEL; } // Set segment deceleration flag + if (st_prep_data->decelerate_after == 0) { prep_segment->flag = RAMP_CHANGE_DECEL; } // Set segment deceleration flag else { st_prep_data->current_approx_rate -= st_prep_data->rate_delta; } if (st_prep_data->current_approx_rate < st_prep_data->rate_delta) { st_prep_data->current_approx_rate >>= 1; } } else { @@ -618,10 +618,12 @@ void st_prep_buffer() } } + // TODO: Look into replacing the following dist_per_step divide with multiplying its inverse to save cycles. + // Compute the number of steps in the prepped segment based on the approximate current rate. - // NOTE: The dist_next_step divide cancels out the INV_TIME_MULTIPLIER and converts the rate value to steps. + // NOTE: The dist_per_step divide cancels out the INV_TIME_MULTIPLIER and converts the rate value to steps. prep_segment->n_step = ceil(max(MINIMUM_STEP_RATE,st_prep_data->current_approx_rate)* - (ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND)/st_prep_data->dist_next_step); + (ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND)/st_prep_data->dist_per_step); // NOTE: Ensures it moves for very slow motions, but MINIMUM_STEP_RATE should always set this too. Perhaps // a compile-time check to see if MINIMUM_STEP_RATE is set high enough is all that is needed. prep_segment->n_step = max(prep_segment->n_step,MINIMUM_STEPS_PER_SEGMENT); @@ -668,7 +670,6 @@ void st_prep_buffer() // printString(" "); } - } } uint8_t st_get_prep_block_index() From 4f9bcde40e3a7cfcc94721d8939ec93f8ceb858f Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Tue, 29 Oct 2013 19:10:39 -0600 Subject: [PATCH 19/73] Merge branch 'dev_2' into dev Conflicts: README.md gcode.c motion_control.c planner.c planner.h protocol.c report.c settings.c settings.h stepper.c stepper.h --- Makefile | 2 +- README.md | 25 ++ config.h | 146 +++----- defaults.h | 30 +- doc/pinmapping.txt | 97 +++++ eeprom.h | 2 +- gcode.c | 332 +++++++++-------- gcode.h | 8 +- limits.c | 71 ++-- limits.h | 8 +- main.c | 9 +- motion_control.c | 129 +++---- motion_control.h | 5 +- nuts_bolts.c | 15 +- nuts_bolts.h | 9 +- pin_map.h | 184 ++++++++++ planner.c | 876 +++++++++++++++++++++++++-------------------- planner.h | 55 +-- planner_old.c | 476 ++++++++++++++++++++++++ planner_old.h | 83 +++++ print.c | 2 +- print.h | 2 + protocol.c | 38 +- protocol.h | 2 +- report.c | 56 +-- report.h | 6 +- serial.c | 12 +- settings.c | 61 ++-- settings.h | 8 +- stepper.c | 664 +++++++++++++++++++++++++--------- stepper.h | 7 +- stepper_old.c | 746 ++++++++++++++++++++++++++++++++++++++ stepper_v0_9.c | 387 ++++++++++++++++++++ 33 files changed, 3524 insertions(+), 1029 deletions(-) create mode 100644 doc/pinmapping.txt create mode 100644 pin_map.h create mode 100644 planner_old.c create mode 100644 planner_old.h create mode 100644 stepper_old.c create mode 100644 stepper_v0_9.c diff --git a/Makefile b/Makefile index 707de73..213be43 100644 --- a/Makefile +++ b/Makefile @@ -84,7 +84,7 @@ main.elf: $(OBJECTS) grbl.hex: main.elf rm -f grbl.hex avr-objcopy -j .text -j .data -O ihex main.elf grbl.hex - avr-size -C --mcu=$(DEVICE) main.elf + avr-size --format=berkeley main.elf # If you have an EEPROM section, you must also create a hex file for the # EEPROM and add it to the "flash" target. diff --git a/README.md b/README.md index 8945d1b..0ed3006 100644 --- a/README.md +++ b/README.md @@ -2,3 +2,28 @@ ------------ This branch serves only as a developmental platform for working on new ideas that may eventually be installed into Grbl's edge branch. Please do not use as there is no guarantee this code base is up-to-date or working. + +------------ + +Grbl is a no-compromise, high performance, low cost alternative to parallel-port-based motion control for CNC milling. It will run on a vanilla Arduino (Duemillanove/Uno) as long as it sports an Atmega 328. + +The controller is written in highly optimized C utilizing every clever feature of the AVR-chips to achieve precise timing and asynchronous operation. It is able to m aintain more than 30kHz of stable, jitter free control pulses. + +It accepts standards-compliant G-code and has been tested with the output of several CAM tools with no problems. Arcs, circles and helical motion are fully supported, as well as, other basic functional g-code commands. Functions and variables are not currently supported, but may be included in future releases in a form of a pre-processor. + +Grbl includes full acceleration management with look ahead. That means the controller will look up to 18 motions into the future and plan its velocities ahead to deliver smooth acceleration and jerk-free cornering. + +##Changelog for v0.9 from v0.8 + - **ALPHA status: Under heavy development.** + - New stepper algorithm: Based on an inverse time algorithm, but modified to ensure steps are executed exactly. This algorithm performs a constant timer tick and has a hard limit of 30kHz maximum step frequency. It is also highly tuneable and should be very easy to port to other microcontroller architectures. Overall, a much better, smoother stepper algorithm with the capability of very high speeds. + - Planner optimizations: Multiple changes to increase planner execution speed and removed redundant variables. + - Acceleration independence: Each axes may be defined with different acceleration parameters and Grbl will automagically calculate the maximum acceleration through a path depending on the direction traveled. This is very useful for machine that have very different axes properties, like the ShapeOko z-axis. + - Maximum velocity independence: As with acceleration, the maximum velocity of individual axes may be defined. All seek/rapids motions will move at these maximum rates, but never exceed any one axes. So, when two or more axes move, the limiting axis will move at its maximum rate, while the other axes are scaled down. + - Significantly improved arc performance: Arcs are now defined in terms of chordal tolerance, rather than segment length. Chordal tolerance will automatically scale all arc line segments depending on arc radius, such that the error does not exceed the tolerance value (default: 0.005 mm.) So, for larger radii arcs, Grbl can move faster through them, because the segments are always longer and the planner has more distance to plan with. + - Soft limits: Checks if any motion command exceeds workspace limits. Alarms out when found. Another safety feature, but, unlike hard limits, position does not get lost, as it forces a feed hold before erroring out. + - New Grbl SIMULATOR by @jgeisler: A completely independent wrapper of the Grbl main source code that may be compiled as an executable on a computer. No Arduino required. Simply simulates the responses of Grbl as if it was on an Arduino. May be used for many things: checking out how Grbl works, pre-process moves for GUI graphics, debugging of new features, etc. Much left to do, but potentially very powerful, as the dummy AVR variables can be written to output anything you need. + - Homing routine updated: Sets workspace volume in all negative space regardless of limit switch position. Common on pro CNCs. Also reduces soft limits CPU overhead. + - Feedrate overrides: In the works, but planner has begun to be re-factored for this feature. + - Jogging controls: Methodology needs to be to figured out first. Could be dropped due to flash space concerns. Last item on the agenda. + +_The project was initially inspired by the Arduino GCode Interpreter by Mike Ellery_ diff --git a/config.h b/config.h index 93fd1ee..9aa82f0 100644 --- a/config.h +++ b/config.h @@ -2,8 +2,8 @@ config.h - compile time configuration Part of Grbl + Copyright (c) 2011-2013 Sungeun K. Jeon Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2011-2012 Sungeun K. 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 @@ -19,80 +19,24 @@ along with Grbl. If not, see . */ -#ifndef config_h -#define config_h +// This file contains compile-time configurations for Grbl's internal system. For the most part, +// users will not need to directly modify these, but they are here for specific needs, i.e. +// performance tuning or adjusting to non-typical machines. // IMPORTANT: Any changes here requires a full re-compiling of the source code to propagate them. +#ifndef config_h +#define config_h + // Default settings. Used when resetting EEPROM. Change to desired name in defaults.h -#define DEFAULTS_GENERIC +#define DEFAULTS_ZEN_TOOLWORKS_7x7 // Serial baud rate -#define BAUD_RATE 9600 +#define BAUD_RATE 115200 -// Define pin-assignments -// NOTE: All step bit and direction pins must be on the same port. -#define STEPPING_DDR DDRD -#define STEPPING_PORT PORTD -#define X_STEP_BIT 2 // Uno Digital Pin 2 -#define Y_STEP_BIT 3 // Uno Digital Pin 3 -#define Z_STEP_BIT 4 // Uno Digital Pin 4 -#define X_DIRECTION_BIT 5 // Uno Digital Pin 5 -#define Y_DIRECTION_BIT 6 // Uno Digital Pin 6 -#define Z_DIRECTION_BIT 7 // Uno Digital Pin 7 -#define STEP_MASK ((1< 255) +#error Parameters ACCELERATION_TICKS / ISR_TICKS must be < 256 to prevent integer overflow. +#endif + +// --------------------------------------------------------------------------------------- #endif diff --git a/defaults.h b/defaults.h index 293e934..7ee6606 100644 --- a/defaults.h +++ b/defaults.h @@ -2,7 +2,7 @@ defaults.h - defaults settings configuration file Part of Grbl - Copyright (c) 2012 Sungeun K. Jeon + Copyright (c) 2012-2013 Sungeun K. 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 @@ -42,6 +42,7 @@ #define DEFAULT_REPORT_INCHES 0 // false #define DEFAULT_AUTO_START 1 // true #define DEFAULT_INVERT_ST_ENABLE 0 // false + #define DEFAULT_SOFT_LIMIT_ENABLE 0 // false #define DEFAULT_HARD_LIMIT_ENABLE 0 // false #define DEFAULT_HOMING_ENABLE 0 // false #define DEFAULT_HOMING_DIR_MASK 0 // move positive dir @@ -49,8 +50,11 @@ #define DEFAULT_HOMING_FEEDRATE 25.0 // mm/min #define DEFAULT_HOMING_DEBOUNCE_DELAY 100 // msec (0-65k) #define DEFAULT_HOMING_PULLOFF 1.0 // mm - #define DEFAULT_STEPPER_IDLE_LOCK_TIME 25 // msec (0-255) + #define DEFAULT_STEPPER_IDLE_LOCK_TIME 25 // msec (0-254, 255 keeps steppers enabled) #define DEFAULT_DECIMAL_PLACES 3 + #define DEFAULT_X_MAX_TRAVEL 200 // mm + #define DEFAULT_Y_MAX_TRAVEL 200 // mm + #define DEFAULT_Z_MAX_TRAVEL 200 // mm #endif #ifdef DEFAULTS_SHERLINE_5400 @@ -72,6 +76,7 @@ #define DEFAULT_REPORT_INCHES 1 // false #define DEFAULT_AUTO_START 1 // true #define DEFAULT_INVERT_ST_ENABLE 0 // false + #define DEFAULT_SOFT_LIMIT_ENABLE 0 // false #define DEFAULT_HARD_LIMIT_ENABLE 0 // false #define DEFAULT_HOMING_ENABLE 0 // false #define DEFAULT_HOMING_DIR_MASK 0 // move positive dir @@ -79,8 +84,11 @@ #define DEFAULT_HOMING_FEEDRATE 25.0 // mm/min #define DEFAULT_HOMING_DEBOUNCE_DELAY 100 // msec (0-65k) #define DEFAULT_HOMING_PULLOFF 1.0 // mm - #define DEFAULT_STEPPER_IDLE_LOCK_TIME 25 // msec (0-255) + #define DEFAULT_STEPPER_IDLE_LOCK_TIME 25 // msec (0-254, 255 keeps steppers enabled) #define DEFAULT_DECIMAL_PLACES 3 + #define DEFAULT_X_MAX_TRAVEL 200 // mm + #define DEFAULT_Y_MAX_TRAVEL 200 // mm + #define DEFAULT_Z_MAX_TRAVEL 200 // mm #endif #ifdef DEFAULTS_SHAPEOKO @@ -105,6 +113,7 @@ #define DEFAULT_REPORT_INCHES 0 // false #define DEFAULT_AUTO_START 1 // true #define DEFAULT_INVERT_ST_ENABLE 0 // false + #define DEFAULT_SOFT_LIMIT_ENABLE 0 // false #define DEFAULT_HARD_LIMIT_ENABLE 0 // false #define DEFAULT_HOMING_ENABLE 0 // false #define DEFAULT_HOMING_DIR_MASK 0 // move positive dir @@ -112,8 +121,11 @@ #define DEFAULT_HOMING_FEEDRATE 25.0 // mm/min #define DEFAULT_HOMING_DEBOUNCE_DELAY 100 // msec (0-65k) #define DEFAULT_HOMING_PULLOFF 1.0 // mm - #define DEFAULT_STEPPER_IDLE_LOCK_TIME 255 // msec (0-255) + #define DEFAULT_STEPPER_IDLE_LOCK_TIME 255 // msec (0-254, 255 keeps steppers enabled) #define DEFAULT_DECIMAL_PLACES 3 + #define DEFAULT_X_MAX_TRAVEL 200 // mm + #define DEFAULT_Y_MAX_TRAVEL 200 // mm + #define DEFAULT_Z_MAX_TRAVEL 200 // mm #endif #ifdef DEFAULTS_ZEN_TOOLWORKS_7x7 @@ -128,14 +140,15 @@ #define DEFAULT_Z_STEPS_PER_MM (STEPS_PER_REV*MICROSTEPS/MM_PER_REV) #define DEFAULT_STEP_PULSE_MICROSECONDS 10 #define DEFAULT_ARC_TOLERANCE 0.005 // mm - #define DEFAULT_RAPID_FEEDRATE 2500.0 // mm/min + #define DEFAULT_RAPID_FEEDRATE 4000.0 // mm/min #define DEFAULT_FEEDRATE 1000.0 // mm/min - #define DEFAULT_ACCELERATION 150.0*60*60 // 150 mm/min^2 + #define DEFAULT_ACCELERATION 400.0*60*60 // 150 mm/min^2 #define DEFAULT_JUNCTION_DEVIATION 0.05 // mm #define DEFAULT_STEPPING_INVERT_MASK (1< N_COORDINATE_SYSTEM)) { // L2 and L20. P1=G54, P2=G55, ... + if ((l != 2 && l != 20) || (int_value < 0 || int_value > N_COORDINATE_SYSTEM)) { // L2 and L20. P1=G54, P2=G55, ... FAIL(STATUS_UNSUPPORTED_STATEMENT); } else if (!axis_words && l==2) { // No axis words. FAIL(STATUS_INVALID_STATEMENT); } else { - int_value--; // Adjust P index to EEPROM coordinate data indexing. - if (l == 20) { - settings_write_coord_data(int_value,gc.position); - // Update system coordinate system if currently active. - if (gc.coord_select == int_value) { memcpy(gc.coord_system,gc.position,sizeof(gc.position)); } - } else { - float coord_data[N_AXIS]; - if (!settings_read_coord_data(int_value,coord_data)) { return(STATUS_SETTING_READ_FAIL); } - // Update axes defined only in block. Always in machine coordinates. Can change non-active system. - uint8_t i; - for (i=0; i 0) { int_value--; } // Adjust P1-P6 index to EEPROM coordinate data indexing. + else { int_value = gc.coord_select; } // Index P0 as the active coordinate system + float coord_data[N_AXIS]; + if (!settings_read_coord_data(int_value,coord_data)) { return(STATUS_SETTING_READ_FAIL); } + // Update axes defined only in block. Always in machine coordinates. Can change non-active system. + for (idx=0; idx C -----------------+--------------- T <- [x,y] - | <------ d/2 ---->| - - C - Current position - T - Target position - O - center of circle that pass through both C and T - d - distance from C to T - r - designated radius - h - distance from center of CT to O - - Expanding the equations: - - d -> sqrt(x^2 + y^2) - h -> sqrt(4 * r^2 - x^2 - y^2)/2 - i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 - j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 - - Which can be written: - - i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2 - j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2 - - Which we for size and speed reasons optimize to: - - h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2) - i = (x - (y * h_x2_div_d))/2 - j = (y + (x * h_x2_div_d))/2 - - */ - - // Calculate the change in position along each selected axis - float x = target[gc.plane_axis_0]-gc.position[gc.plane_axis_0]; - float y = target[gc.plane_axis_1]-gc.position[gc.plane_axis_1]; - - clear_vector(offset); - // First, use h_x2_div_d to compute 4*h^2 to check if it is negative or r is smaller - // than d. If so, the sqrt of a negative number is complex and error out. - float h_x2_div_d = 4 * r*r - x*x - y*y; - if (h_x2_div_d < 0) { FAIL(STATUS_ARC_RADIUS_ERROR); return(gc.status_code); } - // Finish computing h_x2_div_d. - h_x2_div_d = -sqrt(h_x2_div_d)/hypot(x,y); // == -(h * 2 / d) - // Invert the sign of h_x2_div_d if the circle is counter clockwise (see sketch below) - if (gc.motion_mode == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d; } - - /* The counter clockwise circle lies to the left of the target direction. When offset is positive, - the left hand circle will be generated - when it is negative the right hand circle is generated. - - - T <-- Target position - - ^ - Clockwise circles with this center | Clockwise circles with this center will have - will have > 180 deg of angular travel | < 180 deg of angular travel, which is a good thing! - \ | / - center of arc when h_x2_div_d is positive -> x <----- | -----> x <- center of arc when h_x2_div_d is negative - | - | - - C <-- Current position */ - - - // Negative R is g-code-alese for "I want a circle with more than 180 degrees of travel" (go figure!), - // 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; - 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] = 0.5*(x-(y*h_x2_div_d)); - offset[gc.plane_axis_1] = 0.5*(y+(x*h_x2_div_d)); - + if (gc.arc_radius != 0) { // Arc Radius Mode + // Compute arc radius and offsets + gc_convert_arc_radius_mode(target); + if (gc.status_code) { return(gc.status_code); } } else { // Arc Center Format Offset Mode - r = hypot(offset[gc.plane_axis_0], offset[gc.plane_axis_1]); // Compute arc radius for mc_arc + gc.arc_radius = hypot(gc.arc_offset[gc.plane_axis_0], gc.arc_offset[gc.plane_axis_1]); // Compute arc radius for mc_arc } // Set clockwise/counter-clockwise sign for mc_arc computations @@ -519,9 +443,9 @@ uint8_t gc_execute_line(char *line) if (gc.motion_mode == MOTION_MODE_CW_ARC) { isclockwise = true; } // Trace the arc - mc_arc(gc.position, target, offset, gc.plane_axis_0, gc.plane_axis_1, gc.plane_axis_2, + mc_arc(gc.position, target, gc.arc_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, - r, isclockwise); + gc.arc_radius, isclockwise); } break; } @@ -553,7 +477,7 @@ uint8_t gc_execute_line(char *line) // Parses the next statement and leaves the counter on the first character following // the statement. Returns 1 if there was a statements, 0 if end of string was reached // or there was an error (check state.status_code). -static int next_statement(char *letter, float *float_ptr, char *line, uint8_t *char_counter) +static uint8_t next_statement(char *letter, float *float_ptr, char *line, uint8_t *char_counter) { if (line[*char_counter] == 0) { return(0); // No more statements @@ -572,6 +496,100 @@ static int next_statement(char *letter, float *float_ptr, char *line, uint8_t *c return(1); } + +static void gc_convert_arc_radius_mode(float *target) +{ +/* We need to calculate the center of the circle that has the designated radius and passes + through both the current position and the target position. This method calculates the following + set of equations where [x,y] is the vector from current to target position, d == magnitude of + that vector, h == hypotenuse of the triangle formed by the radius of the circle, the distance to + the center of the travel vector. A vector perpendicular to the travel vector [-y,x] is scaled to the + length of h [-y/d*h, x/d*h] and added to the center of the travel vector [x/2,y/2] to form the new point + [i,j] at [x/2-y/d*h, y/2+x/d*h] which will be the center of our arc. + + d^2 == x^2 + y^2 + h^2 == r^2 - (d/2)^2 + i == x/2 - y/d*h + j == y/2 + x/d*h + + O <- [i,j] + - | + r - | + - | + - | h + - | + [0,0] -> C -----------------+--------------- T <- [x,y] + | <------ d/2 ---->| + + C - Current position + T - Target position + O - center of circle that pass through both C and T + d - distance from C to T + r - designated radius + h - distance from center of CT to O + + Expanding the equations: + + d -> sqrt(x^2 + y^2) + h -> sqrt(4 * r^2 - x^2 - y^2)/2 + i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 + j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 + + Which can be written: + + i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2 + j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2 + + Which we for size and speed reasons optimize to: + + h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2) + i = (x - (y * h_x2_div_d))/2 + j = (y + (x * h_x2_div_d))/2 */ + + // Calculate the change in position along each selected axis + float x = target[gc.plane_axis_0]-gc.position[gc.plane_axis_0]; + float y = target[gc.plane_axis_1]-gc.position[gc.plane_axis_1]; + + clear_vector(gc.arc_offset); + // First, use h_x2_div_d to compute 4*h^2 to check if it is negative or r is smaller + // than d. If so, the sqrt of a negative number is complex and error out. + float h_x2_div_d = 4 * gc.arc_radius*gc.arc_radius - x*x - y*y; + if (h_x2_div_d < 0) { FAIL(STATUS_ARC_RADIUS_ERROR); return; } + // Finish computing h_x2_div_d. + h_x2_div_d = -sqrt(h_x2_div_d)/hypot(x,y); // == -(h * 2 / d) + // Invert the sign of h_x2_div_d if the circle is counter clockwise (see sketch below) + if (gc.motion_mode == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d; } + + /* The counter clockwise circle lies to the left of the target direction. When offset is positive, + the left hand circle will be generated - when it is negative the right hand circle is generated. + + + T <-- Target position + + ^ + Clockwise circles with this center | Clockwise circles with this center will have + will have > 180 deg of angular travel | < 180 deg of angular travel, which is a good thing! + \ | / + center of arc when h_x2_div_d is positive -> x <----- | -----> x <- center of arc when h_x2_div_d is negative + | + | + + C <-- Current position */ + + + // Negative R is g-code-alese for "I want a circle with more than 180 degrees of travel" (go figure!), + // 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 (gc.arc_radius < 0) { + h_x2_div_d = -h_x2_div_d; + gc.arc_radius = -gc.arc_radius; // Finished with r. Set to positive for mc_arc + } + // Complete the operation by calculating the actual center of the arc + gc.arc_offset[gc.plane_axis_0] = 0.5*(x-(y*h_x2_div_d)); + gc.arc_offset[gc.plane_axis_1] = 0.5*(y+(x*h_x2_div_d)); +} + /* Not supported: diff --git a/gcode.h b/gcode.h index 50b4228..6b8949a 100644 --- a/gcode.h +++ b/gcode.h @@ -82,7 +82,11 @@ typedef struct { float coord_system[N_AXIS]; // Current work coordinate system (G54+). Stores offset from absolute machine // position in mm. Loaded from EEPROM when called. float coord_offset[N_AXIS]; // Retains the G92 coordinate offset (work coordinates) relative to - // machine zero in mm. Non-persistent. Cleared upon reset and boot. + // machine zero in mm. Non-persistent. Cleared upon reset and boot. + + float arc_radius; + float arc_offset[N_AXIS]; + } parser_state_t; extern parser_state_t gc; @@ -93,6 +97,6 @@ void gc_init(); uint8_t gc_execute_line(char *line); // Set g-code parser position. Input in steps. -void gc_set_current_position(int32_t x, int32_t y, int32_t z); +void gc_sync_position(); #endif diff --git a/limits.c b/limits.c index 91180ce..3b15353 100644 --- a/limits.c +++ b/limits.c @@ -3,7 +3,7 @@ Part of Grbl Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2012 Sungeun K. Jeon + Copyright (c) 2012-2013 Sungeun K. 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 @@ -59,12 +59,6 @@ void limits_init() // your e-stop switch to the Arduino reset pin, since it is the most correct way to do this. ISR(LIMIT_INT_vect) { - // TODO: This interrupt may be used to manage the homing cycle directly with the main stepper - // interrupt without adding too much to it. All it would need is some way to stop one axis - // when its limit is triggered and continue the others. This may reduce some of the code, but - // would make Grbl a little harder to read and understand down road. Holding off on this until - // we move on to new hardware or flash space becomes an issue. If it ain't broke, don't fix it. - // Ignore limit switches if already in an alarm state or in-process of executing an alarm. // When in the alarm state, Grbl should have been reset or will force a reset, so any pending // moves in the planner and serial buffers are all cleared and newly sent blocks will be @@ -90,6 +84,19 @@ ISR(LIMIT_INT_vect) // NOTE: Only the abort runtime command can interrupt this process. static void homing_cycle(uint8_t cycle_mask, int8_t pos_dir, bool invert_pin, float homing_rate) { + + /* TODO: Change homing routine to call planner instead moving at the maximum seek rates + and (max_travel+10mm?) for each axes during the search phase. The routine should monitor + the state of the limit pins and when a pin is triggered, it can disable that axes by + setting the respective step_x, step_y, or step_z value in the executing planner block. + This keeps the stepper algorithm counters from triggering the step on that particular + axis. When all axes have been triggered, we can then disable the steppers and reset + the stepper and planner buffers. This same method can be used for the locate cycles. + This will also fix the slow max feedrate of the homing 'lite' stepper algorithm. + + Need to check if setting the planner steps will require them to be volatile or not. */ + + // Determine governing axes with finest step resolution per distance for the Bresenham // algorithm. This solves the issue when homing multiple axes that have different // resolutions without exceeding system acceleration setting. It doesn't have to be @@ -97,22 +104,17 @@ static void homing_cycle(uint8_t cycle_mask, int8_t pos_dir, bool invert_pin, fl // and speedy homing routine. // NOTE: For each axes enabled, the following calculations assume they physically move // an equal distance over each time step until they hit a limit switch, aka dogleg. + uint32_t step_event_count = 0; + uint8_t i, dist = 0; uint32_t steps[N_AXIS]; - uint8_t dist = 0; clear_vector(steps); - if (cycle_mask & (1< 0 || target[idx] < settings.max_travel[idx]) { // NOTE: max_travel is stored as negative + + // Force feed hold if cycle is active. All buffered blocks are guaranteed to be within + // workspace volume so just come to a controlled stop so position is not lost. When complete + // enter alarm mode. + if (sys.state == STATE_CYCLE) { + st_feed_hold(); + while (sys.state == STATE_HOLD) { + protocol_execute_runtime(); + if (sys.abort) { return; } + } + } + + mc_reset(); // Issue system reset and ensure spindle and coolant are shutdown. + sys.execute |= EXEC_CRIT_EVENT; // Indicate soft limit critical event + protocol_execute_runtime(); // Execute to enter critical event loop and system abort + return; + + } + } +} diff --git a/limits.h b/limits.h index 847c667..ac94dd6 100644 --- a/limits.h +++ b/limits.h @@ -3,6 +3,7 @@ Part of Grbl Copyright (c) 2009-2011 Simen Svale Skogsrud + Copyright (c) 2013 Sungeun K. 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 @@ -21,10 +22,13 @@ #ifndef limits_h #define limits_h -// initialize the limits module +// Initialize the limits module void limits_init(); -// perform the homing cycle +// Perform the homing cycle void limits_go_home(); +// Check for soft limit violations +void limits_soft_check(float *target); + #endif \ No newline at end of file diff --git a/main.c b/main.c index b1e7734..9decae8 100644 --- a/main.c +++ b/main.c @@ -3,7 +3,7 @@ Part of Grbl Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2011-2012 Sungeun K. Jeon + Copyright (c) 2011-2013 Sungeun K. 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 @@ -72,7 +72,8 @@ int main(void) // Sync cleared gcode and planner positions to current system position, which is only // cleared upon startup, not a reset/abort. - sys_sync_current_position(); + plan_sync_position(); + gc_sync_position(); // Reset system variables. sys.abort = false; @@ -101,12 +102,12 @@ int main(void) } protocol_execute_runtime(); - protocol_process(); // ... process the serial protocol // When the serial protocol returns, there are no more characters in the serial read buffer to // be processed and executed. This indicates that individual commands are being issued or // streaming is finished. In either case, auto-cycle start, if enabled, any queued moves. - if (sys.auto_start) { st_cycle_start(); } + mc_auto_cycle_start(); + protocol_process(); // ... process the serial protocol } return 0; /* never reached */ diff --git a/motion_control.c b/motion_control.c index 0834940..d417031 100644 --- a/motion_control.c +++ b/motion_control.c @@ -3,7 +3,7 @@ Part of Grbl Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2011-2012 Sungeun K. Jeon + Copyright (c) 2011-2013 Sungeun K. Jeon Copyright (c) 2011 Jens Geisler Grbl is free software: you can redistribute it and/or modify @@ -42,41 +42,15 @@ // (1 minute)/feed_rate time. // NOTE: This is the primary gateway to the grbl planner. All line motions, including arc line // segments, must pass through this routine before being passed to the planner. The seperation of -// mc_line and plan_buffer_line is done primarily to make backlash compensation or canned cycle -// integration simple and direct. -// TODO: Check for a better way to avoid having to push the arguments twice for non-backlash cases. -// However, this keeps the memory requirements lower since it doesn't have to call and hold two -// plan_buffer_lines in memory. Grbl only has to retain the original line input variables during a -// backlash segment(s). +// mc_line and plan_buffer_line is done primarily to place non-planner-type functions from being +// in the planner and to let backlash compensation or canned cycle integration simple and direct. void mc_line(float *target, float feed_rate, uint8_t invert_feed_rate) { - // TO TEST: Perform soft limit check here. Just check if the target x,y,z values are outside the - // work envelope. Should be straightforward and efficient. By placing it here, rather than in - // the g-code parser, it directly picks up motions from everywhere in Grbl. - // TODO: Eventually move the soft limit check into limits.c. - if (bit_istrue(settings.flags,BITFLAG_SOFT_LIMIT_ENABLE)) { - uint8_t i; - for (i=0; i settings.max_travel[i])) { - // TODO: Need to make this more in-line with the rest of the alarm and runtime execution handling. - // Not quite right. Also this should force Grbl to feed hold and exit, rather than stopping and alarm - // out. This would help retain machine position, but is this really required? - if (sys.state != STATE_ALARM) { - if (bit_isfalse(sys.execute,EXEC_ALARM)) { - mc_reset(); // Initiate system kill. - report_alarm_message(ALARM_SOFT_LIMIT); - sys.state = STATE_ALARM; - sys.execute |= EXEC_CRIT_EVENT; // Indicate hard limit critical event - } - } - } - } - } - - // If in check gcode mode, prevent motion by blocking planner. + // If enabled, check for soft limit violations. Placed here all line motions are picked up + // from everywhere in Grbl. + if (bit_istrue(settings.flags,BITFLAG_SOFT_LIMIT_ENABLE)) { limits_soft_check(target); } + + // If in check gcode mode, prevent motion by blocking planner. Soft limits still work. if (sys.state == STATE_CHECK_MODE) { return; } // TODO: Backlash compensation may be installed here. Only need direction info to track when @@ -85,31 +59,25 @@ void mc_line(float *target, float feed_rate, uint8_t invert_feed_rate) // backlash steps will need to be also tracked. Not sure what the best strategy is for this, // i.e. keep the planner independent and do the computations in the status reporting, or let // the planner handle the position corrections. The latter may get complicated. + // TODO: Backlash comp positioning values may need to be kept at a system level, i.e. tracking + // true position after a feed hold in the middle of a backlash move. The difficulty is in making + // sure that the stepper subsystem and planner are working in sync, and the status report + // position also takes this into account. // If the buffer is full: good! That means we are well ahead of the robot. // Remain in this loop until there is room in the buffer. do { protocol_execute_runtime(); // Check for any run-time commands if (sys.abort) { return; } // Bail, if system abort. - } while ( plan_check_full_buffer() ); - plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], feed_rate, invert_feed_rate); + if ( plan_check_full_buffer() ) { mc_auto_cycle_start(); } // Auto-cycle start when buffer is full. + else { break; } + } while (1); + + plan_buffer_line(target, feed_rate, invert_feed_rate); // If idle, indicate to the system there is now a planned block in the buffer ready to cycle // start. Otherwise ignore and continue on. if (!sys.state) { sys.state = STATE_QUEUED; } - - // Auto-cycle start immediately after planner finishes. Enabled/disabled by grbl settings. During - // a feed hold, auto-start is disabled momentarily until the cycle is resumed by the cycle-start - // runtime command. - // NOTE: This is allows the user to decide to exclusively use the cycle start runtime command to - // begin motion or let grbl auto-start it for them. This is useful when: manually cycle-starting - // when the buffer is completely full and primed; auto-starting, if there was only one g-code - // command sent during manual operation; or if a system is prone to buffer starvation, auto-start - // helps make sure it minimizes any dwelling/motion hiccups and keeps the cycle going. - // NOTE: Moved into main loop and plan_check_full_buffer() as a test. This forces Grbl to process - // all of the commands in the serial read buffer or until the planner buffer is full before auto - // cycle starting. Will eventually need to remove the following command. - // if (sys.auto_start) { st_cycle_start(); } } @@ -181,8 +149,8 @@ void mc_arc(float *position, float *target, float *offset, uint8_t axis_0, uint8 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.16666667*(cos_T + 4); + float cos_T = 2.0 - theta_per_segment*theta_per_segment; + float sin_T = theta_per_segment*0.16666667*(cos_T + 4.0); cos_T *= 0.5; float arc_target[N_AXIS]; @@ -256,32 +224,40 @@ void mc_go_home() protocol_execute_runtime(); // Check for reset and set system abort. if (sys.abort) { return; } // Did not complete. Alarm state set by mc_alarm. - // The machine should now be homed and machine zero has been located. Upon completion, - // reset system position and sync internal position vectors. - clear_vector_float(sys.position); // Set machine zero - sys_sync_current_position(); - sys.state = STATE_IDLE; // Set system state to IDLE to complete motion and indicate homed. - - // Pull-off axes (that have been homed) from limit switches before continuing motion. + // The machine should now be homed and machine limits have been located. By default, + // grbl defines machine space as all negative, as do most CNCs. Since limit switches + // can be on either side of an axes, check and set machine zero appropriately. + // At the same time, set up pull-off maneuver from axes limit switches that have been homed. // This provides some initial clearance off the switches and should also help prevent them // from falsely tripping when hard limits are enabled. - float target[N_AXIS]; - target[X_AXIS] = target[Y_AXIS] = target[Z_AXIS] = settings.homing_pulloff; - if (HOMING_LOCATE_CYCLE & (1< #include "config.h" #include "defaults.h" +#include "pin_map.h" #define false 0 #define true 1 #define N_AXIS 3 // Number of axes -#define X_AXIS 0 // Axis indexing value +#define X_AXIS 0 // Axis indexing value. Must start with 0 and be continuous. #define Y_AXIS 1 #define Z_AXIS 2 @@ -44,6 +45,7 @@ // Useful macros #define clear_vector(a) memset(a, 0, sizeof(a)) #define clear_vector_float(a) memset(a, 0.0, sizeof(float)*N_AXIS) +#define clear_vector_long(a) memset(a, 0.0, sizeof(long)*N_AXIS) #define max(a,b) (((a) > (b)) ? (a) : (b)) #define min(a,b) (((a) < (b)) ? (a) : (b)) @@ -104,7 +106,6 @@ void delay_ms(uint16_t ms); // Delays variable-defined microseconds. Compiler compatibility fix for _delay_us(). void delay_us(uint32_t us); -// Syncs Grbl's gcode and planner position variables with the system position. -void sys_sync_current_position(); +uint8_t get_direction_mask(uint8_t i); #endif diff --git a/pin_map.h b/pin_map.h new file mode 100644 index 0000000..233b32a --- /dev/null +++ b/pin_map.h @@ -0,0 +1,184 @@ +/* + pin_map.h - Pin mapping configuration file + Part of Grbl + + Copyright (c) 2013 Sungeun K. 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 + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +/* The pin_map.h file serves as a central pin mapping settings file for different processor + types, i.e. AVR 328p or AVR Mega 2560. Grbl officially supports the Arduino Uno, but the + other supplied pin mappings are supplied by users, so your results may vary. */ + +#ifndef pin_map_h +#define pin_map_h + +#ifdef PIN_MAP_ARDUINO_UNO // AVR 328p, Officially supported by Grbl. + + // Serial port pins + #define SERIAL_RX USART_RX_vect + #define SERIAL_UDRE USART_UDRE_vect + + // NOTE: All step bit and direction pins must be on the same port. + #define STEPPING_DDR DDRD + #define STEPPING_PORT PORTD + #define X_STEP_BIT 2 // Uno Digital Pin 2 + #define Y_STEP_BIT 3 // Uno Digital Pin 3 + #define Z_STEP_BIT 4 // Uno Digital Pin 4 + #define X_DIRECTION_BIT 5 // Uno Digital Pin 5 + #define Y_DIRECTION_BIT 6 // Uno Digital Pin 6 + #define Z_DIRECTION_BIT 7 // Uno Digital Pin 7 + #define STEP_MASK ((1< + - +--------+ <- nominal_rate /|\ - / \ / | \ - initial_rate -> + \ / | + <- next->initial_rate - | + <- next->initial_rate / | | - +-------------+ initial_rate -> +----+--+ - time --> ^ ^ ^ ^ - | | | | - decelerate distance decelerate distance - - Calculates trapezoid parameters for stepper algorithm. Each block velocity profiles can be - described as either a trapezoidal or a triangular shape. The trapezoid occurs when the block - reaches the nominal speed of the block and cruises for a period of time. A triangle occurs - when the nominal speed is not reached within the block. Some other special cases exist, - such as pure ac/de-celeration velocity profiles from beginning to end or a trapezoid that - has no deceleration period when the next block resumes acceleration. - - The following function determines the type of velocity profile and stores the minimum required - information for the stepper algorithm to execute the calculated profiles. In this case, only - the new initial rate and n_steps until deceleration are computed, since the stepper algorithm - already handles acceleration and cruising and just needs to know when to start decelerating. -*/ -static uint8_t calculate_trapezoid_for_block(block_t *block, uint8_t idx, float entry_speed_sqr, float exit_speed_sqr) -{ - // Compute new initial rate for stepper algorithm - // volatile is necessary so that the optimizer doesn't move the calculation in the ATOMIC_BLOCK - volatile uint32_t initial_rate = ceil(sqrt(entry_speed_sqr)*(RANADE_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) - - // TODO: Compute new nominal rate if a feedrate override occurs. Could be performed by simple scalar. - // block->nominal_rate = ceil(feed_rate*(RANADE_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) - - // Compute efficiency variable for following calculations. Removes a float divide and multiply. - // TODO: If memory allows, this can be kept in the block buffer since it doesn't change, even after feed holds. - float steps_per_mm_div_2_acc = block->step_event_count/(2*block->acceleration*block->millimeters); - - // First determine intersection distance (in steps) from the exit point for a triangular profile. - // Computes: steps_intersect = steps/mm * ( distance/2 + (v_entry^2-v_exit^2)/(4*acceleration) ) - int32_t intersect_distance = ceil( 0.5*(block->step_event_count + steps_per_mm_div_2_acc*(entry_speed_sqr-exit_speed_sqr)) ); - - // Check if this is a pure acceleration block by a intersection distance less than zero. Also - // prevents signed and unsigned integer conversion errors. - uint32_t decelerate_after= 0; - if (intersect_distance > 0) { - // Determine deceleration distance (in steps) from nominal speed to exit speed for a trapezoidal profile. - // Value is never negative. Nominal speed is always greater than or equal to the exit speed. - // Computes: steps_decelerate = steps/mm * ( (v_nominal^2 - v_exit^2)/(2*acceleration) ) - decelerate_after = ceil(steps_per_mm_div_2_acc * (block->nominal_speed_sqr - exit_speed_sqr)); + // !!! block index is the same as block_buffer_safe. + // See if we can reduce this down to just requesting the millimeters remaining.. + uint8_t is_decelerating; + float millimeters_remaining = 0.0; + st_fetch_partial_block_parameters(block_index, &millimeters_remaining, &is_decelerating); + + if (millimeters_remaining != 0.0) { + // Point to current block partially executed by stepper algorithm + plan_block_t *partial_block = plan_get_block_by_index(block_index); - // The lesser of the two triangle and trapezoid distances always defines the velocity profile. - if (decelerate_after > intersect_distance) { decelerate_after = intersect_distance; } - - // Finally, check if this is a pure deceleration block. - if (decelerate_after > block->step_event_count) { decelerate_after = block->step_event_count; } - } - - uint8_t block_buffer_tail_hold= block_buffer_tail; // store to avoid rereading volatile - // check if we got overtaken by the stepper - if(idx==prev_block_index(block_buffer_tail_hold)) { - return false; - } - - // check where the stepper is currently working relative to the block we want to update - uint8_t block_buffer_tail_next= next_block_index(block_buffer_tail_hold); - if(idx==block_buffer_tail_hold || idx==block_buffer_tail_next) { - // we are close to were the stepper is working, so we need to block it for a short time - // to safely adjust the block - - // I counted the cycles in this block from the assembler code - // It's 42 cycles worst case including the call to st_is_decelerating - // @ 16MHz this is 2.6250e-06 seconds, 30kHz cycle duration is 3.3333e-05 seconds - // -> this block will delay the stepper timer by max 8% - // given that this occurs not very often, it should be ok - // but test will have to show - - // ATOMIC_BLOCK only works with compiler parameter --std=c99 - ATOMIC_BLOCK(ATOMIC_FORCEON) { - // reload block_buffer_tail in case it changed - uint8_t block_buffer_tail_hold2= block_buffer_tail; - if(idx!=block_buffer_tail_hold2) { - if(block_buffer_tail_hold2==block_buffer_tail_next) - return false; // the stepper didn't overtook in the meantime - } else { - if(st_is_decelerating()) - return false; // we want to change the currently running block and it has already started to decelerate - } - - block->decelerate_after= decelerate_after; - block->initial_rate= initial_rate; - return true; + // Compute the midway speed of the partially completely block at the end of the segment buffer. + if (is_decelerating) { // Block is decelerating + partial_block->entry_speed_sqr = exit_speed_sqr - 2*partial_block->acceleration*millimeters_remaining; + } else { // Block is accelerating or cruising + partial_block->entry_speed_sqr += 2*partial_block->acceleration*(partial_block->millimeters-millimeters_remaining); + partial_block->entry_speed_sqr = min(partial_block->entry_speed_sqr, partial_block->nominal_speed_sqr); } - } else { - // let's assume the stepper did not complete two blocks since we loaded block_buffer_tail to block_buffer_tail_hold - // so the block we want to change is not currently being run by the stepper and it's safe to touch it without precautions - block->decelerate_after= decelerate_after; - block->initial_rate= initial_rate; - return true; - } - return false; -} - + // Update only the relevant planner block information so the planner can plan correctly. + partial_block->millimeters = millimeters_remaining; + partial_block->max_entry_speed_sqr = partial_block->entry_speed_sqr; // Not sure if this needs to be updated. + } +} + /* PLANNER SPEED DEFINITION +--------+ <- current->nominal_speed / \ current->entry_speed -> + \ - | + <- next->entry_speed + | + <- next->entry_speed (aka exit speed) +-------------+ time --> - Recalculates the motion plan according to the following algorithm: + Recalculates the motion plan according to the following basic guidelines: - 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 speed is equal to or less than the maximum junction speed limit - b. No speed reduction within one block requires faster deceleration than the acceleration limits. - c. The last (or newest appended) block is planned from a complete stop. + 1. Go over every feasible block sequentially in reverse order and calculate the junction speeds + (i.e. current->entry_speed) such that: + a. No junction speed exceeds the pre-computed maximum junction speed limit or nominal speeds of + neighboring blocks. + b. A block entry speed cannot exceed one reverse-computed from its exit speed (next->entry_speed) + with a maximum allowable deceleration over the block travel distance. + c. The last (or newest appended) block is planned from a complete stop (an exit speed of zero). 2. Go over every block in chronological (forward) order and dial down junction speed values if - a. The speed increase within one block would require faster acceleration than the acceleration limits. + a. The exit speed exceeds the one forward-computed from its entry speed with the maximum allowable + acceleration over the block travel distance. - When these stages are complete, all blocks have a junction entry speed that will allow all speed changes - to be performed using the overall limiting acceleration value, and where no junction speed is greater - than the max limit. In other words, it just computed the fastest possible velocity profile through all - buffered blocks, where the final buffered block is planned to come to a full stop when the buffer is fully - executed. Finally it will: + When these stages are complete, the planner will have maximized the velocity profiles throughout the all + of the planner blocks, where every block is operating at its maximum allowable acceleration limits. In + other words, for all of the blocks in the planner, the plan is optimal and no further speed improvements + are possible. If a new block is added to the buffer, the plan is recomputed according to the said + guidelines for a new optimal plan. - 3. Convert the plan to data that the stepper algorithm needs. Only block trapezoids adjacent to a - a planner-modified junction speed with be updated, the others are assumed ok as is. + To increase computational efficiency of these guidelines, a set of planner block pointers have been + created to indicate stop-compute points for when the planner guidelines cannot logically make any further + changes or improvements to the plan when in normal operation and new blocks are streamed and added to the + planner buffer. For example, if a subset of sequential blocks in the planner have been planned and are + bracketed by junction velocities at their maximums (or by the first planner block as well), no new block + added to the planner buffer will alter the velocity profiles within them. So we no longer have to compute + them. Or, if a set of sequential blocks from the first block in the planner (or a optimal stop-compute + point) are all accelerating, they are all optimal and can not be altered by a new block added to the + planner buffer, as this will only further increase the plan speed to chronological blocks until a maximum + junction velocity is reached. However, if the operational conditions of the plan changes from infrequently + used feed holds or feedrate overrides, the stop-compute pointers will be reset and the entire plan is + recomputed as stated in the general guidelines. - All planner computations(1)(2) are performed in floating point to minimize numerical round-off errors. Only - when planned values are converted to stepper rate parameters(3), these are integers. If another motion block - is added while executing, the planner will re-plan and update the stored optimal velocity profile as it goes. + Planner buffer index mapping: + - block_buffer_tail: Points to the beginning of the planner buffer. First to be executed or being executed. + - block_buffer_head: Points to the buffer block after the last block in the buffer. Used to indicate whether + the buffer is full or empty. As described for standard ring buffers, this block is always empty. + - next_buffer_head: Points to next planner buffer block after the buffer head block. When equal to the + buffer tail, this indicates the buffer is full. + - block_buffer_safe: Points to the first sequential planner block for which it is safe to recompute, which + is defined to be where the stepper's step segment buffer ends. This may or may not be the buffer tail, + since the step segment buffer queues steps which may have not finished executing and could span a few + blocks, if the block moves are very short. + - block_buffer_planned: Points to the first buffer block after the last optimally planned block for normal + streaming operating conditions. Use for planning optimizations by avoiding recomputing parts of the + planner buffer that don't change with the addition of a new block, as describe above. - Conceptually, the planner works like blowing up a balloon, where the balloon is the velocity profile. It's - constrained by the speeds at the beginning and end of the buffer, along with the maximum junction speeds and - nominal speeds of each block. Once a plan is computed, or balloon filled, this is the optimal velocity profile - through all of the motions in the buffer. Whenever a new block is added, this changes some of the limiting - conditions, or how the balloon is filled, so it has to be re-calculated to get the new optimal velocity profile. + NOTE: All planner computations are performed in floating point to minimize numerical round-off errors. + When a planner block is executed, the floating point values are converted to fast integers by the stepper + algorithm segment buffer. See the stepper module for details. - Also, since the planner only computes on what's in the planner buffer, some motions with lots of short line - segments, like arcs, may seem to move slow. This is because there simply isn't enough combined distance traveled - in the entire buffer to accelerate up to the nominal speed and then decelerate to a stop at the end of the - buffer. There are a few simple solutions to this: (1) Maximize the machine acceleration. The planner will be - able to compute higher speed profiles within the same combined distance. (2) Increase line segment(s) distance. - The more combined distance the planner has to use, the faster it can go. (3) Increase the MINIMUM_PLANNER_SPEED. - Not recommended. This will change what speed the planner plans to at the end of the buffer. Can lead to lost - steps when coming to a stop. (4) [BEST] Increase the planner buffer size. The more combined distance, the - bigger the balloon, or faster it can go. But this is not possible for 328p Arduinos because its limited memory - is already maxed out. Future ARM versions should not have this issue, with look-ahead planner blocks numbering - up to a hundred or more. + NOTE: Since the planner only computes on what's in the planner buffer, some motions with lots of short + line segments, like G2/3 arcs or complex curves, may seem to move slow. This is because there simply isn't + enough combined distance traveled in the entire buffer to accelerate up to the nominal speed and then + decelerate to a complete stop at the end of the buffer, as stated by the guidelines. If this happens and + becomes an annoyance, there are a few simple solutions: (1) Maximize the machine acceleration. The planner + will be able to compute higher velocity profiles within the same combined distance. (2) Maximize line + segment(s) distance per block to a desired tolerance. The more combined distance the planner has to use, + the faster it can go. (3) Maximize the planner buffer size. This also will increase the combined distance + for the planner to compute over. It also increases the number of computations the planner has to perform + to compute an optimal plan, so select carefully. The Arduino 328p memory is already maxed out, but future + ARM versions should have enough memory and speed for look-ahead blocks numbering up to a hundred or more. - NOTE: Since this function is constantly re-calculating for every new incoming block, it must be as efficient - as possible. For example, in situations like arc generation or complex curves, the short, rapid line segments - can execute faster than new blocks can be added, and the planner buffer will then starve and empty, leading - to weird hiccup-like jerky motions. */ -static uint8_t planner_recalculate() -{ - uint8_t current_block_idx= block_buffer_head; - block_t *curr_block = &block_buffer[current_block_idx]; - uint8_t plan_unchanged= 1; - - if(current_block_idx!=block_buffer_tail) { // we cannot do anything to only one block - float max_entry_speed_sqr; - float next_entry_speed_sqr= 0.0; - // loop backwards to possibly postpone deceleration - while(current_block_idx!=planned_block_tail) { // the second block is the one where we start the forward loop - if(current_block_idx==block_buffer_tail) { - planned_block_tail= current_block_idx; - break; - } +static void planner_recalculate() +{ - // TODO: Determine maximum entry speed at junction for feedrate overrides, since they can alter - // the planner nominal speeds at any time. This calc could be done in the override handler, but - // this could require an additional variable to be stored to differentiate the programmed nominal - // speeds, max junction speed, and override speeds/scalar. + // Initialize block index to the last block in the planner buffer. + uint8_t block_index = plan_prev_block_index(block_buffer_head); - // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising. - // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and - // check for maximum allowable speed reductions to ensure maximum possible planned speed. - if (curr_block->entry_speed_sqr >= curr_block->max_entry_speed_sqr) { - // default if next_entry_speed_sqr > curr_block->max_entry_speed_sqr || max_entry_speed_sqr > curr_block->max_entry_speed_sqr - curr_block->new_entry_speed_sqr = curr_block->max_entry_speed_sqr; + // Query stepper module for safe planner block index to recalculate to, which corresponds to the end + // of the step segment buffer. + uint8_t block_buffer_safe = st_get_prep_block_index(); + + // TODO: Make sure that we don't have to check for the block_buffer_tail condition, if the stepper module + // returns a NULL pointer or something. This could happen when the segment buffer is empty. Although, + // this call won't return a NULL, only an index.. I have to make sure that this index is synced with the + // planner at all times. + + // Recompute plan only when there is more than one planner block in the buffer. Can't do anything with one. + // NOTE: block_buffer_safe can be the last planner block if the segment buffer has completely queued up the + // remainder of the planner buffer. In this case, a new planner block will be treated as a single block. + if (block_index == block_buffer_safe) { // Also catches (head-1) = tail + + // Just set block_buffer_planned pointer. + block_buffer_planned = block_index; - if (next_entry_speed_sqr < curr_block->max_entry_speed_sqr) { - // Computes: v_entry^2 = v_exit^2 + 2*acceleration*distance - max_entry_speed_sqr = next_entry_speed_sqr + 2*curr_block->acceleration*curr_block->millimeters; - if (max_entry_speed_sqr < curr_block->max_entry_speed_sqr) { - curr_block->new_entry_speed_sqr = max_entry_speed_sqr; + // TODO: Feedrate override of one block needs to update the partial block with an exit speed of zero. For + // a single added block and recalculate after a feed hold, we don't need to compute this, since we already + // know that the velocity starts and ends at zero. With an override, we can be traveling at some midblock + // rate, and we have to calculate the new velocity profile from it. + // plan_update_partial_block(block_index,0.0); + + } else { + + // TODO: If the nominal speeds change during a feedrate override, we need to recompute the max entry speeds for + // all junctions before proceeding. + + // Initialize planner buffer pointers and indexing. + plan_block_t *current = &block_buffer[block_index]; + + // Calculate maximum entry speed for last block in buffer, where the exit speed is always zero. + current->entry_speed_sqr = min( current->max_entry_speed_sqr, 2*current->acceleration*current->millimeters); + + // Reverse Pass: Coarsely maximize all possible deceleration curves back-planning from the last + // block in buffer. Cease planning when: (1) the last optimal planned pointer is reached. + // (2) the safe block pointer is reached, whereby the planned pointer is updated. + // NOTE: Forward pass will later refine and correct the reverse pass to create an optimal plan. + // NOTE: If the safe block is encountered before the planned block pointer, we know the safe block + // will be recomputed within the plan. So, we need to update it if it is partially completed. + float entry_speed_sqr; + plan_block_t *next; + block_index = plan_prev_block_index(block_index); + + if (block_index == block_buffer_safe) { // !! OR plan pointer? Yes I think so. + + // Only two plannable blocks in buffer. Compute previous block based on + // !!! May only work if a new block is being added. Not for an override. The exit speed isn't zero. + // !!! Need to make the current entry speed calculation after this. + plan_update_partial_block(block_index, 0.0); + block_buffer_planned = block_index; + + } else { + + // Three or more plan-able + while (block_index != block_buffer_planned) { + + next = current; + current = &block_buffer[block_index]; + + // Increment block index early to check if the safe block is before the current block. If encountered, + // this is an exit condition as we can't go further than this block in the reverse pass. + block_index = plan_prev_block_index(block_index); + if (block_index == block_buffer_safe) { + // Check if the safe block is partially completed. If so, update it before its exit speed + // (=current->entry speed) is over-written. + // TODO: The update breaks with feedrate overrides, because the replanning process no longer has + // the previous nominal speed to update this block with. There will need to be something along the + // lines of a nominal speed change check and send the correct value to this function. + plan_update_partial_block(block_index,current->entry_speed_sqr); + + // Set planned pointer at safe block and for loop exit after following computation is done. + block_buffer_planned = block_index; + } + + // Compute maximum entry speed decelerating over the current block from its exit speed. + if (current->entry_speed_sqr != current->max_entry_speed_sqr) { + entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters; + if (entry_speed_sqr < current->max_entry_speed_sqr) { + current->entry_speed_sqr = entry_speed_sqr; + } else { + current->entry_speed_sqr = current->max_entry_speed_sqr; } } - } else { - curr_block->new_entry_speed_sqr = curr_block->entry_speed_sqr; } - next_entry_speed_sqr= curr_block->new_entry_speed_sqr; + + } - current_block_idx= prev_block_index( current_block_idx ); - curr_block= &block_buffer[current_block_idx]; + // Forward Pass: Forward plan the acceleration curve from the planned pointer onward. + // Also scans for optimal plan breakpoints and appropriately updates the planned pointer. + next = &block_buffer[block_buffer_planned]; // Begin at buffer planned pointer + block_index = plan_next_block_index(block_buffer_planned); + while (block_index != block_buffer_head) { + current = next; + next = &block_buffer[block_index]; + + // Any acceleration detected in the forward pass automatically moves the optimal planned + // pointer forward, since everything before this is all optimal. In other words, nothing + // can improve the plan from the buffer tail to the planned pointer by logic. + // TODO: Need to check if the planned flag logic is correct for all scenarios. It may not + // be for certain conditions. However, if the block reaches nominal speed, it can be a valid + // breakpoint substitute. + if (current->entry_speed_sqr < next->entry_speed_sqr) { + entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters; + // If true, current block is full-acceleration and we can move the planned pointer forward. + if (entry_speed_sqr < next->entry_speed_sqr) { + next->entry_speed_sqr = entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass sets this. + block_buffer_planned = block_index; // Set optimal plan pointer. + } + } + + // Any block set at its maximum entry speed also creates an optimal plan up to this + // point in the buffer. When the plan is bracketed by either the beginning of the + // buffer and a maximum entry speed or two maximum entry speeds, every block in between + // cannot logically be further improved. Hence, we don't have to recompute them anymore. + if (next->entry_speed_sqr == next->max_entry_speed_sqr) { + block_buffer_planned = block_index; // Set optimal plan pointer + } + + block_index = plan_next_block_index( block_index ); } - // loop forward, adjust exit speed to not exceed max accelleration - block_t *next_block; - uint8_t next_block_idx; - float max_exit_speed_sqr; - while(current_block_idx!=block_buffer_head) { - next_block_idx= next_block_index(current_block_idx); - next_block = &block_buffer[next_block_idx]; - - // If the current 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 exit speed accordingly. Entry - // speeds have already been reset, maximized, and reverse planned by reverse planner. - if (curr_block->entry_speed_sqr < next_block->new_entry_speed_sqr) { - // Compute block exit speed based on the current block speed and distance - // Computes: v_exit^2 = v_entry^2 + 2*acceleration*distance - max_exit_speed_sqr = curr_block->entry_speed_sqr + 2*curr_block->acceleration*curr_block->millimeters; - - } else { - max_exit_speed_sqr= SOME_LARGE_VALUE; - } - - // adjust max_exit_speed_sqr in case this is a deceleration block or max accel cannot be reached - if(max_exit_speed_sqr>next_block->new_entry_speed_sqr) { - max_exit_speed_sqr= next_block->new_entry_speed_sqr; - } else { - // this block has reached max acceleration, it is optimal - planned_block_tail= next_block_idx; - } - - if(calculate_trapezoid_for_block(curr_block, current_block_idx, curr_block->entry_speed_sqr, max_exit_speed_sqr)) { - next_block->entry_speed_sqr= max_exit_speed_sqr; - plan_unchanged= 0; - } else if(!plan_unchanged) { // we started to modify the plan an then got overtaken by the stepper executing the plan: this is bad - return(0); - } - - // Check if the next block entry speed is at max_entry_speed. If so, move the planned pointer, since - // this entry speed cannot be improved anymore and all prior blocks have been completed and optimally planned. - if(next_block->entry_speed_sqr>=next_block->max_entry_speed_sqr) { - planned_block_tail= next_block_idx; - } - - current_block_idx= next_block_idx; - curr_block= next_block; - } - } + } - if(!calculate_trapezoid_for_block(curr_block, current_block_idx, curr_block->entry_speed_sqr, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED)) { - // this can only happen to the first block in the queue? so we dont need to clear or stop anything - return(0); - } else - return(1); } + +void plan_reset_buffer() +{ + block_buffer_planned = block_buffer_tail; +} + + void plan_init() { - block_buffer_tail = block_buffer_head= planned_block_tail; - next_buffer_head = next_block_index(block_buffer_head); -// block_buffer_planned = block_buffer_head; + block_buffer_tail = 0; + block_buffer_head = 0; // Empty = tail + next_buffer_head = 1; // plan_next_block_index(block_buffer_head) + plan_reset_buffer(); memset(&pl, 0, sizeof(pl)); // Clear planner struct } -inline void plan_discard_current_block() + +void plan_discard_current_block() { - if (block_buffer_head != block_buffer_tail) { - block_buffer_tail = next_block_index( block_buffer_tail ); + if (block_buffer_head != block_buffer_tail) { // Discard non-empty buffer. + block_buffer_tail = plan_next_block_index( block_buffer_tail ); } } -inline block_t *plan_get_current_block() + +plan_block_t *plan_get_current_block() { - if (block_buffer_head == block_buffer_tail) { return(NULL); } + if (block_buffer_head == block_buffer_tail) { // Buffer empty + plan_reset_buffer(); + return(NULL); + } return(&block_buffer[block_buffer_tail]); } + +plan_block_t *plan_get_block_by_index(uint8_t block_index) +{ + if (block_buffer_head == block_index) { return(NULL); } + return(&block_buffer[block_index]); +} + + // Returns the availability status of the block ring buffer. True, if full. uint8_t plan_check_full_buffer() { - if (block_buffer_tail == next_buffer_head) { - // TODO: Move this back into motion control. Shouldn't be here, but it's efficient. - if (sys.auto_start) { st_cycle_start(); } // Auto-cycle start when buffer is full. - return(true); - } + if (block_buffer_tail == next_buffer_head) { return(true); } return(false); } + // Block until all buffered steps are executed or in a cycle state. Works with feed hold // during a synchronize call, if it should happen. Also, waits for clean cycle end. void plan_synchronize() @@ -372,178 +374,229 @@ void plan_synchronize() } } -// Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in -// millimeters. 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. -// All position data passed to the planner must be in terms of machine position to keep the planner -// independent of any coordinate system changes and offsets, which are handled by the g-code parser. -// NOTE: Assumes buffer is available. Buffer checks are handled at a higher level by motion_control. -// Also the feed rate input value is used in three ways: as a normal feed rate if invert_feed_rate -// is false, as inverse time if invert_feed_rate is true, or as seek/rapids rate if the feed_rate -// value is negative (and invert_feed_rate always false). -void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert_feed_rate) + +/* Add a new linear movement to the buffer. target[N_AXIS] is the signed, absolute target position + in millimeters. 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. + All position data passed to the planner must be in terms of machine position to keep the planner + independent of any coordinate system changes and offsets, which are handled by the g-code parser. + NOTE: Assumes buffer is available. Buffer checks are handled at a higher level by motion_control. + In other words, the buffer head is never equal to the buffer tail. Also the feed rate input value + is used in three ways: as a normal feed rate if invert_feed_rate is false, as inverse time if + invert_feed_rate is true, or as seek/rapids rate if the feed_rate value is negative (and + invert_feed_rate always false). */ +void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) { - // Prepare to set up new block - block_t *block = &block_buffer[block_buffer_head]; + // Prepare and initialize new block + plan_block_t *block = &block_buffer[block_buffer_head]; + block->step_event_count = 0; + block->millimeters = 0; + block->direction_bits = 0; + block->acceleration = SOME_LARGE_VALUE; // Scaled down to maximum acceleration later - // Calculate target position in absolute steps - int32_t target[N_AXIS]; - target[X_AXIS] = lround(x*settings.steps_per_mm[X_AXIS]); - target[Y_AXIS] = lround(y*settings.steps_per_mm[Y_AXIS]); - target[Z_AXIS] = lround(z*settings.steps_per_mm[Z_AXIS]); + // Compute and store initial move distance data. + // TODO: After this for-loop, we don't touch the stepper algorithm data. Might be a good idea + // to try to keep these types of things completely separate from the planner for portability. + int32_t target_steps[N_AXIS]; + float unit_vec[N_AXIS], delta_mm; + uint8_t idx; + for (idx=0; idxsteps_x = labs(target[X_AXIS]-pl.position[X_AXIS]); - block->steps_y = labs(target[Y_AXIS]-pl.position[Y_AXIS]); - block->steps_z = labs(target[Z_AXIS]-pl.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; }; + // Number of steps for each axis and determine max step events + block->steps[idx] = labs(target_steps[idx]-pl.position[idx]); + block->step_event_count = max(block->step_event_count, block->steps[idx]); + + // Compute individual axes distance for move and prep unit vector calculations. + // NOTE: Computes true distance from converted step values. + delta_mm = (target_steps[idx] - pl.position[idx])/settings.steps_per_mm[idx]; + unit_vec[idx] = delta_mm; // Store unit vector numerator. Denominator computed later. + + // Set direction bits. Bit enabled always means direction is negative. + if (delta_mm < 0 ) { block->direction_bits |= get_direction_mask(idx); } + + // Incrementally compute total move distance by Euclidean norm. First add square of each term. + block->millimeters += delta_mm*delta_mm; + } + block->millimeters = sqrt(block->millimeters); // Complete millimeters calculation with sqrt() + + // Bail if this is a zero-length block. Highly unlikely to occur. + if (block->step_event_count == 0) { return; } - // Compute path vector in terms of absolute step target and current positions - // NOTE: Operates by arithmetic rather than expensive division. - float delta_mm[N_AXIS]; - delta_mm[X_AXIS] = x-pl.last_x; - delta_mm[Y_AXIS] = y-pl.last_y; - delta_mm[Z_AXIS] = z-pl.last_z; - block->millimeters = sqrt(delta_mm[X_AXIS]*delta_mm[X_AXIS] + delta_mm[Y_AXIS]*delta_mm[Y_AXIS] + - delta_mm[Z_AXIS]*delta_mm[Z_AXIS]); - // Adjust feed_rate value to mm/min depending on type of rate input (normal, inverse time, or rapids) // TODO: Need to distinguish a rapids vs feed move for overrides. Some flag of some sort. if (feed_rate < 0) { feed_rate = SOME_LARGE_VALUE; } // Scaled down to absolute max/rapids rate later else if (invert_feed_rate) { feed_rate = block->millimeters/feed_rate; } - // Calculate the unit vector of the line move and the block maximum feed rate and acceleration limited - // by the maximum possible values. Block rapids rates are computed or feed rates are scaled down so - // they don't exceed the maximum axes velocities. The block acceleration is maximized based on direction - // and axes properties as well. + // Calculate the unit vector of the line move and the block maximum feed rate and acceleration scaled + // down such that no individual axes maximum values are exceeded with respect to the line direction. // NOTE: This calculation assumes all axes are orthogonal (Cartesian) and works with ABC-axes, // if they are also orthogonal/independent. Operates on the absolute value of the unit vector. - uint8_t i; - float unit_vec[N_AXIS], inverse_unit_vec_value; + float inverse_unit_vec_value; float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple float divides - block->acceleration = SOME_LARGE_VALUE; // Scaled down to maximum acceleration in loop - for (i=0; iacceleration = min(block->acceleration,settings.acceleration[i]*inverse_unit_vec_value); + float junction_cos_theta = 0; + for (idx=0; idxacceleration = min(block->acceleration,settings.acceleration[idx]*inverse_unit_vec_value); + + // Incrementally compute cosine of angle between previous and current path. Cos(theta) of the junction + // between the current move and the previous move is simply the dot product of the two unit vectors, + // where prev_unit_vec is negative. Used later to compute maximum junction speed. + junction_cos_theta -= pl.previous_unit_vec[idx] * unit_vec[idx]; } } - // Compute nominal speed - block->nominal_speed_sqr = feed_rate*feed_rate; // (mm/min)^2. Always > 0 - - // Pre-calculate stepper algorithm values: Acceleration rate, distance traveled per step event, and nominal rate. - // TODO: Obsolete? Sort of. This pre-calculates this value so the stepper algorithm doesn't have to upon loading. - // The multiply and ceil() may take too many cycles but removing it would save (BUFFER_SIZE-1)*4 bytes of memory. - block->nominal_rate = ceil(feed_rate*(RANADE_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) - block->rate_delta = ceil(block->acceleration* - ((RANADE_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) - block->d_next = ceil((block->millimeters*RANADE_MULTIPLIER)/block->step_event_count); // (mult*mm/step) + // TODO: Need to check this method handling zero junction speeds when starting from rest. + if (block_buffer_head == block_buffer_tail) { - // Compute direction bits. Bit enabled always means direction is negative. - // TODO: Check if this can be combined with steps_x calcs to speed up. Not sure though since - // this only has to perform a negative check on already existing values. I think I've measured - // the speed difference. This should be optimal in speed and flash space, I believe. - block->direction_bits = 0; - if (unit_vec[X_AXIS] < 0) { block->direction_bits |= (1<direction_bits |= (1<direction_bits |= (1<entry_speed_sqr = 0.0; + block->max_junction_speed_sqr = 0.0; // Starting from rest. Enforce start from zero velocity. + + } else { + /* + Compute maximum allowable entry speed at junction by centripetal acceleration approximation. + 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 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. This approach does not actually deviate + from path, but used as a robust way to compute cornering speeds, as it takes into account the + nonlinearities of both the junction angle and junction velocity. - // Compute maximum allowable entry speed at junction by centripetal acceleration approximation. - // 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 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. This approach does not actually deviate - // from path, but used as a robust way to compute cornering speeds, as it takes into account the - // nonlinearities of both the junction angle and junction velocity. - // NOTE: If the junction deviation value is finite, Grbl executes the motions in an exact path - // mode (G61). If the junction deviation value is zero, Grbl will execute the motion in an exact - // stop mode (G61.1) manner. In the future, if continuous mode (G64) is desired, the math here - // is exactly the same. Instead of motioning all the way to junction point, the machine will - // just follow the arc circle defined here. The Arduino doesn't have the CPU cycles to perform - // a continuous mode path, but ARM-based microcontrollers most certainly do. + NOTE: If the junction deviation value is finite, Grbl executes the motions in an exact path + mode (G61). If the junction deviation value is zero, Grbl will execute the motion in an exact + stop mode (G61.1) manner. In the future, if continuous mode (G64) is desired, the math here + is exactly the same. Instead of motioning all the way to junction point, the machine will + just follow the arc circle defined here. The Arduino doesn't have the CPU cycles to perform + a continuous mode path, but ARM-based microcontrollers most certainly do. + + NOTE: The max junction speed is a fixed value, since machine acceleration limits cannot be + changed dynamically during operation nor can the line move geometry. This must be kept in + memory in the event of a feedrate override changing the nominal speeds of blocks, which can + change the overall maximum entry speed conditions of all blocks. + */ + // NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta). + float sin_theta_d2 = sqrt(0.5*(1.0-junction_cos_theta)); // Trig half angle identity. Always positive. - block->max_entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_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) && (pl.previous_nominal_speed_sqr > 0.0)) { - // 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. - float cos_theta = - pl.previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] - - pl.previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] - - pl.previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ; - - // Skip and use default max junction speed for 0 degree acute junction. - if (cos_theta < 0.95) { - block->max_entry_speed_sqr = min(block->nominal_speed_sqr,pl.previous_nominal_speed_sqr); - // Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds. - if (cos_theta > -0.95) { - // Compute maximum junction velocity based on maximum acceleration and junction deviation - float sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive. - block->max_entry_speed_sqr = min(block->max_entry_speed_sqr, - block->acceleration * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)); - } - } - } - - // Initialize block entry speed - // TODO: Check if this is computed in the recalculate function automatically. Although this - // never changes since this already computed as the optimum. - block->entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED; + // TODO: Acceleration used in calculation needs to be limited by the minimum of the two junctions. + block->max_junction_speed_sqr = max( MINIMUM_JUNCTION_SPEED*MINIMUM_JUNCTION_SPEED, + (block->acceleration * settings.junction_deviation * sin_theta_d2)/(1.0-sin_theta_d2) ); + } + // Store block nominal speed + block->nominal_speed_sqr = feed_rate*feed_rate; // (mm/min). Always > 0 + + // Compute the junction maximum entry based on the minimum of the junction speed and neighboring nominal speeds. + block->max_entry_speed_sqr = min(block->max_junction_speed_sqr, + min(block->nominal_speed_sqr,pl.previous_nominal_speed_sqr)); + // Update previous path unit_vector and nominal speed (squared) memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // pl.previous_unit_vec[] = unit_vec[] pl.previous_nominal_speed_sqr = block->nominal_speed_sqr; // Update planner position - memcpy(pl.position, target, sizeof(target)); // pl.position[] = target[] - pl.last_x = x; - pl.last_y = y; - pl.last_z = z; + memcpy(pl.position, target_steps, sizeof(target_steps)); // pl.position[] = target_steps[] - if(!planner_recalculate()) { - // TODO: make alarm informative - if (sys.state != STATE_ALARM) { - if (bit_isfalse(sys.execute,EXEC_ALARM)) { - mc_reset(); // Initiate system kill. - sys.execute |= EXEC_CRIT_EVENT; // Indicate hard limit critical event - } - } - } - - // Update buffer head and next buffer head indices - // NOTE: Mind that updating block_buffer_head after the planner changes the planner logic a bit - // TODO: Check if this is better to place after recalculate or before in terms of buffer executing. + // New block is all set. Update buffer head and next buffer head indices. block_buffer_head = next_buffer_head; - next_buffer_head = next_block_index(block_buffer_head); + next_buffer_head = plan_next_block_index(block_buffer_head); + + // Finish up by recalculating the plan with the new block. + planner_recalculate(); + +// int32_t blength = block_buffer_head - block_buffer_tail; +// if (blength < 0) { blength += BLOCK_BUFFER_SIZE; } +// printInteger(blength); + + } + // Reset the planner position vectors. Called by the system abort/initialization routine. -void plan_set_current_position(int32_t x, int32_t y, int32_t z) +void plan_sync_position() { - pl.position[X_AXIS] = x; - pl.position[Y_AXIS] = y; - pl.position[Z_AXIS] = z; - pl.last_x = x/settings.steps_per_mm[X_AXIS]; - pl.last_y = y/settings.steps_per_mm[Y_AXIS]; - pl.last_z = z/settings.steps_per_mm[Z_AXIS]; + uint8_t idx; + for (idx=0; idx + + +--------+ <- nominal_speed /|\ + / \ / | \ + entry_speed -> + \ / | + <- next->entry_speed + | + <- next->entry_speed / | | + +-------------+ entry_speed -> +----+--+ + time --> ^ ^ ^ ^ + | | | | + decelerate distance decelerate distance + + Calculates the type of velocity profile for a given planner block and provides the deceleration + distance for the stepper algorithm to use to accurately trace the profile exactly. The planner + computes the entry and exit speeds of each block, but does not bother to determine the details of + the velocity profiles within them, as they aren't needed for computing an optimal plan. When the + stepper algorithm begins to execute a block, the block velocity profiles are computed ad hoc. + + Each block velocity profiles can be described as either a trapezoidal or a triangular shape. The + trapezoid occurs when the block reaches the nominal speed of the block and cruises for a period of + time. A triangle occurs when the nominal speed is not reached within the block. Both of these + velocity profiles may also be truncated on either end with no acceleration or deceleration ramps, + as they can be influenced by the conditions of neighboring blocks, where the acceleration ramps + are defined by constant acceleration equal to the maximum allowable acceleration of a block. + + Since the stepper algorithm already assumes to begin executing a planner block by accelerating + from the planner entry speed and cruise if the nominal speed is reached, we only need to know + when to begin deceleration to the end of the block. Hence, only the distance from the end of the + block to begin a deceleration ramp is computed for the stepper algorithm when requested. +*/ +float plan_calculate_velocity_profile(uint8_t block_index) +{ + plan_block_t *current_block = &block_buffer[block_index]; + + // Determine current block exit speed + float exit_speed_sqr = 0.0; // Initialize for end of planner buffer. Zero speed. + plan_block_t *next_block = plan_get_block_by_index(plan_next_block_index(block_index)); + if (next_block != NULL) { exit_speed_sqr = next_block->entry_speed_sqr; } // Exit speed is the entry speed of next buffer block + + // First determine intersection distance (in steps) from the exit point for a triangular profile. + // Computes: d_intersect = distance/2 + (v_entry^2-v_exit^2)/(4*acceleration) + float intersect_distance = 0.5*( current_block->millimeters + (current_block->entry_speed_sqr-exit_speed_sqr)/(2*current_block->acceleration) ); + + // Check if this is a pure acceleration block by a intersection distance less than zero. Also + // prevents signed and unsigned integer conversion errors. + if (intersect_distance > 0 ) { + float decelerate_distance; + // Determine deceleration distance (in steps) from nominal speed to exit speed for a trapezoidal profile. + // Value is never negative. Nominal speed is always greater than or equal to the exit speed. + // Computes: d_decelerate = (v_nominal^2 - v_exit^2)/(2*acceleration) + decelerate_distance = (current_block->nominal_speed_sqr - exit_speed_sqr)/(2*current_block->acceleration); + + // The lesser of the two triangle and trapezoid distances always defines the velocity profile. + if (decelerate_distance > intersect_distance) { decelerate_distance = intersect_distance; } + + // Finally, check if this is a pure deceleration block. + if (decelerate_distance > current_block->millimeters) { return(0.0); } + else { return( (current_block->millimeters-decelerate_distance) ); } + } + return( current_block->millimeters ); // No deceleration in velocity profile. +} + + // Re-initialize buffer plan with a partially completed block, assumed to exist at the buffer tail. // Called after a steppers have come to a complete stop for a feed hold and the cycle is stopped. void plan_cycle_reinitialize(int32_t step_events_remaining) { - block_t *block = &block_buffer[block_buffer_tail]; // Point to partially completed block + plan_block_t *block = &block_buffer[block_buffer_tail]; // Point to partially completed block // Only remaining millimeters and step_event_count need to be updated for planner recalculate. // Other variables (step_x, step_y, step_z, rate_delta, etc.) all need to remain the same to @@ -551,8 +604,69 @@ void plan_cycle_reinitialize(int32_t step_events_remaining) block->millimeters = (block->millimeters*step_events_remaining)/block->step_event_count; block->step_event_count = step_events_remaining; - // Re-plan from a complete stop. Reset planner entry speeds and flags. + // Re-plan from a complete stop. Reset planner entry speeds and buffer planned pointer. block->entry_speed_sqr = 0.0; block->max_entry_speed_sqr = 0.0; + block_buffer_planned = block_buffer_tail; planner_recalculate(); } + + +/* +TODO: + When a feed hold or feedrate override is reduced, the velocity profile must execute a + deceleration over the existing plan. By logic, since the plan already decelerates to zero + at the end of the buffer, any replanned deceleration mid-way will never exceed this. It + will only asymptotically approach this in the worst case scenario. + + - For a feed hold, we simply need to plan and compute the stopping point within a block + when velocity decelerates to zero. We then can recompute the plan with the already + existing partial block planning code and set the system to a QUEUED state. + - When a feed hold is initiated, the main program should be able to continue doing what + it has been, i.e. arcs, parsing, but needs to be able to reinitialize the plan after + it has come to a stop. + + - For a feed rate override (reduce-only), we need to enforce a deceleration until we + intersect the reduced nominal speed of a block after it's been planned with the new + overrides and the newly planned block is accelerating or cruising only. If the new plan + block is decelerating at the intersection point, we keep decelerating until we find a + valid intersection point. Once we find this point, we can then resume onto the new plan, + but we may need to adjust the deceleration point in the intersection block since the + feedrate override could have intersected at an acceleration ramp. This would change the + acceleration ramp to a cruising, so the deceleration point will have changed, but the + plan will have not. It should still be valid for the rest of the buffer. Coding this + can get complicated, but it should be doable. One issue could be is in how to handle + scenarios when a user issues several feedrate overrides and inundates this code. Does + this method still work and is robust enough to compute all of this on the fly? This is + the critical question. However, we could block user input until the planner has time to + catch to solve this as well. + + - When the feed rate override increases, we don't have to do anything special. We just + replan the entire buffer with the new nominal speeds and adjust the maximum junction + speeds accordingly. + +void plan_compute_deceleration() { + +} + + +void plan_recompute_max_junction_velocity() { + // Assumes the nominal_speed_sqr values have been updated. May need to just multiply + // override values here. + // PROBLEM: Axes-limiting velocities get screwed up. May need to store an int8 value for the + // max override value possible for each block when the line is added. So the nominal_speed + // is computed with that ceiling, but still retained if the rates change again. + uint8_t block_index = block_buffer_tail; + plan_block_t *block = &block_buffer[block_index]; + pl.previous_nominal_speed_sqr = block->nominal_speed_sqr; + block_index = plan_next_block_index(block_index); + while (block_index != block_buffer_head) { + block = &block_buffer[block_index]; + block->max_entry_speed_sqr = min(block->max_junction_speed_sqr, + min(block->nominal_speed_sqr,pl.previous_nominal_speed_sqr)); + pl.previous_nominal_speed_sqr = block->nominal_speed_sqr; + block_index = plan_next_block_index(block_index); + } +} + +*/ diff --git a/planner.h b/planner.h index ff83016..a52a4f5 100644 --- a/planner.h +++ b/planner.h @@ -2,8 +2,8 @@ planner.h - buffers movement commands and manages the acceleration profile plan Part of Grbl + Copyright (c) 2011-2013 Sungeun K. Jeon Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2011-2012 Sungeun K. 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 @@ -21,7 +21,8 @@ #ifndef planner_h #define planner_h - +#include "nuts_bolts.h" + // The number of linear motions that can be in the plan at any give time #ifndef BLOCK_BUFFER_SIZE #define BLOCK_BUFFER_SIZE 18 @@ -32,43 +33,47 @@ typedef struct { // Fields used by the bresenham algorithm for tracing the line - uint8_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) - uint32_t steps_x, steps_y, steps_z; // Step count along each axis - int32_t step_event_count; // The number of step events required to complete this block + // NOTE: Do not change any of these values once set. The stepper algorithm uses them to execute the block correctly. + uint8_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) + int32_t steps[N_AXIS]; // Step count along each axis + int32_t step_event_count; // The maximum step axis count and number of steps required to complete this block. // Fields used by the motion planner to manage acceleration - float nominal_speed_sqr; // The nominal speed for this block in mm/min - float entry_speed_sqr; // Entry speed at previous-current block junction in mm/min - float max_entry_speed_sqr; // Maximum allowable junction entry speed in mm/min - float new_entry_speed_sqr; // Temporary entry speed used by the planner - float millimeters; // The total travel of this block in mm - float acceleration; - - // Settings for the trapezoid generator - uint32_t initial_rate; // The step rate at start of block - int32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive) - uint32_t decelerate_after; // The index of the step event on which to start decelerating - uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute - uint32_t d_next; // Scaled distance to next step -} block_t; + float entry_speed_sqr; // The current planned entry speed at block junction in (mm/min)^2 + float max_entry_speed_sqr; // Maximum allowable entry speed based on the minimum of junction limit and + // neighboring nominal speeds with overrides in (mm/min)^2 + float max_junction_speed_sqr; // Junction entry speed limit based on direction vectors in (mm/min)^2 + float nominal_speed_sqr; // Axis-limit adjusted nominal speed for this block in (mm/min)^2 + float acceleration; // Axis-limit adjusted line acceleration in mm/min^2 + float millimeters; // The remaining distance for this block to be executed in mm + +} plan_block_t; -// Initialize the motion plan subsystem +// Initialize the motion plan subsystem 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 +// Add a new linear movement to the buffer. target[N_AXIS] is the signed, absolute target position +// in millimeters. 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(float x, float y, float z, float feed_rate, uint8_t invert_feed_rate); +void plan_buffer_line(float *target, float 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. void plan_discard_current_block(); // Gets the current block. Returns NULL if buffer empty -block_t *plan_get_current_block(); +plan_block_t *plan_get_current_block(); + +uint8_t plan_next_block_index(uint8_t block_index); + +plan_block_t *plan_get_block_by_index(uint8_t block_index); + +float plan_calculate_velocity_profile(uint8_t block_index); + +// void plan_update_partial_block(uint8_t block_index, float millimeters_remaining, uint8_t is_decelerating); // Reset the planner position vector (in steps) -void plan_set_current_position(int32_t x, int32_t y, int32_t z); +void plan_sync_position(); // Reinitialize plan with a partially completed block void plan_cycle_reinitialize(int32_t step_events_remaining); diff --git a/planner_old.c b/planner_old.c new file mode 100644 index 0000000..1cf6fb4 --- /dev/null +++ b/planner_old.c @@ -0,0 +1,476 @@ +/* + planner.c - buffers movement commands and manages the acceleration profile plan + Part of Grbl + + Copyright (c) 2011-2013 Sungeun K. Jeon + Copyright (c) 2009-2011 Simen Svale Skogsrud + Copyright (c) 2011 Jens Geisler + + 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 + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +/* The ring buffer implementation gleaned from the wiring_serial library by David A. Mellis. */ + +#include +#include +#include "planner.h" +#include "nuts_bolts.h" +#include "stepper.h" +#include "settings.h" +#include "config.h" +#include "protocol.h" + +#define SOME_LARGE_VALUE 1.0E+38 // Used by rapids and acceleration maximization calculations. Just needs + // to be larger than any feasible (mm/min)^2 or mm/sec^2 value. + +static block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instructions +static volatile uint8_t block_buffer_tail; // Index of the block to process now +static uint8_t block_buffer_head; // Index of the next block to be pushed +static uint8_t next_buffer_head; // Index of the next buffer head +static uint8_t block_buffer_planned; // Index of the optimally planned block + +// Define planner variables +typedef struct { + int32_t position[N_AXIS]; // The planner position of the tool in absolute steps. Kept separate + // from g-code position for movements requiring multiple line motions, + // i.e. arcs, canned cycles, and backlash compensation. + float previous_unit_vec[N_AXIS]; // Unit vector of previous path line segment + float previous_nominal_speed_sqr; // Nominal speed of previous path line segment + float last_target[N_AXIS]; // Target position of previous path line segment +} planner_t; +static planner_t pl; + + +// Returns the index of the next block in the ring buffer +// NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication. +static uint8_t next_block_index(uint8_t block_index) +{ + block_index++; + if (block_index == BLOCK_BUFFER_SIZE) { block_index = 0; } + return(block_index); +} + + +// Returns the index of the previous block in the ring buffer +static uint8_t prev_block_index(uint8_t block_index) +{ + if (block_index == 0) { block_index = BLOCK_BUFFER_SIZE; } + block_index--; + return(block_index); +} + + +/* STEPPER VELOCITY PROFILE DEFINITION + less than nominal rate-> + + +--------+ <- nominal_rate /|\ + / \ / | \ + initial_rate -> + \ / | + <- next->initial_rate + | + <- next->initial_rate / | | + +-------------+ initial_rate -> +----+--+ + time --> ^ ^ ^ ^ + | | | | + decelerate distance decelerate distance + + Calculates trapezoid parameters for stepper algorithm. Each block velocity profiles can be + described as either a trapezoidal or a triangular shape. The trapezoid occurs when the block + reaches the nominal speed of the block and cruises for a period of time. A triangle occurs + when the nominal speed is not reached within the block. Some other special cases exist, + such as pure ac/de-celeration velocity profiles from beginning to end or a trapezoid that + has no deceleration period when the next block resumes acceleration. + + The following function determines the type of velocity profile and stores the minimum required + information for the stepper algorithm to execute the calculated profiles. In this case, only + the new initial rate and n_steps until deceleration are computed, since the stepper algorithm + already handles acceleration and cruising and just needs to know when to start decelerating. +*/ +static void calculate_trapezoid_for_block(block_t *block, float entry_speed_sqr, float exit_speed_sqr) +{ + // Compute new initial rate for stepper algorithm + block->initial_rate = ceil(sqrt(entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + + // TODO: Compute new nominal rate if a feedrate override occurs. + // block->nominal_rate = ceil(feed_rate*(RANADE_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + + // Compute efficiency variable for following calculations. Removes a float divide and multiply. + // TODO: If memory allows, this can be kept in the block buffer since it doesn't change, even after feed holds. + float steps_per_mm_div_2_acc = block->step_event_count/(2*block->acceleration*block->millimeters); + + // First determine intersection distance (in steps) from the exit point for a triangular profile. + // Computes: steps_intersect = steps/mm * ( distance/2 + (v_entry^2-v_exit^2)/(4*acceleration) ) + int32_t intersect_distance = ceil( 0.5*(block->step_event_count + steps_per_mm_div_2_acc*(entry_speed_sqr-exit_speed_sqr)) ); + + // Check if this is a pure acceleration block by a intersection distance less than zero. Also + // prevents signed and unsigned integer conversion errors. + if (intersect_distance <= 0) { + block->decelerate_after = 0; + } else { + // Determine deceleration distance (in steps) from nominal speed to exit speed for a trapezoidal profile. + // Value is never negative. Nominal speed is always greater than or equal to the exit speed. + // Computes: steps_decelerate = steps/mm * ( (v_nominal^2 - v_exit^2)/(2*acceleration) ) + block->decelerate_after = ceil(steps_per_mm_div_2_acc * (block->nominal_speed_sqr - exit_speed_sqr)); + + // The lesser of the two triangle and trapezoid distances always defines the velocity profile. + if (block->decelerate_after > intersect_distance) { block->decelerate_after = intersect_distance; } + + // Finally, check if this is a pure deceleration block. + if (block->decelerate_after > block->step_event_count) { block->decelerate_after = block->step_event_count; } + } +} + + +/* PLANNER SPEED DEFINITION + +--------+ <- current->nominal_speed + / \ + current->entry_speed -> + \ + | + <- next->entry_speed + +-------------+ + time --> + + 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 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 acceleration limits. + c. The last (or newest appended) block is planned from a complete stop. + 2. Go over every block in chronological (forward) order and dial down junction speed values if + a. The speed increase within one block would require faster acceleration than the acceleration limits. + + When these stages are complete, all blocks have a junction entry speed that will allow all speed changes + to be performed using the overall limiting acceleration value, and where no junction speed is greater + than the max limit. In other words, it just computed the fastest possible velocity profile through all + buffered blocks, where the final buffered block is planned to come to a full stop when the buffer is fully + executed. Finally it will: + + 3. Convert the plan to data that the stepper algorithm needs. Only block trapezoids adjacent to a + a planner-modified junction speed with be updated, the others are assumed ok as is. + + All planner computations(1)(2) are performed in floating point to minimize numerical round-off errors. Only + when planned values are converted to stepper rate parameters(3), these are integers. If another motion block + is added while executing, the planner will re-plan and update the stored optimal velocity profile as it goes. + + Conceptually, the planner works like blowing up a balloon, where the balloon is the velocity profile. It's + constrained by the speeds at the beginning and end of the buffer, along with the maximum junction speeds and + nominal speeds of each block. Once a plan is computed, or balloon filled, this is the optimal velocity profile + through all of the motions in the buffer. Whenever a new block is added, this changes some of the limiting + conditions, or how the balloon is filled, so it has to be re-calculated to get the new optimal velocity profile. + + Also, since the planner only computes on what's in the planner buffer, some motions with lots of short line + segments, like arcs, may seem to move slow. This is because there simply isn't enough combined distance traveled + in the entire buffer to accelerate up to the nominal speed and then decelerate to a stop at the end of the + buffer. There are a few simple solutions to this: (1) Maximize the machine acceleration. The planner will be + able to compute higher speed profiles within the same combined distance. (2) Increase line segment(s) distance. + The more combined distance the planner has to use, the faster it can go. (3) Increase the MINIMUM_PLANNER_SPEED. + Not recommended. This will change what speed the planner plans to at the end of the buffer. Can lead to lost + steps when coming to a stop. (4) [BEST] Increase the planner buffer size. The more combined distance, the + bigger the balloon, or faster it can go. But this is not possible for 328p Arduinos because its limited memory + is already maxed out. Future ARM versions should not have this issue, with look-ahead planner blocks numbering + up to a hundred or more. + + NOTE: Since this function is constantly re-calculating for every new incoming block, it must be as efficient + as possible. For example, in situations like arc generation or complex curves, the short, rapid line segments + can execute faster than new blocks can be added, and the planner buffer will then starve and empty, leading + to weird hiccup-like jerky motions. +*/ +static void planner_recalculate() +{ + // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated. + uint8_t block_index = block_buffer_head; + block_t *current = &block_buffer[block_index]; // Set as last/newest block in buffer + + // Determine safe point for which to plan to. + uint8_t block_buffer_safe = next_block_index( block_buffer_tail ); + + if (block_buffer_safe == next_buffer_head) { // Only one safe block in buffer to operate on + + block_buffer_planned = block_buffer_safe; + calculate_trapezoid_for_block(current, 0.0, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED); + + } else { + + // TODO: need to account for the two block condition better. If the currently executing block + // is not safe, do we wait until its done? Can we treat the buffer head differently? + + // Calculate trapezoid for the last/newest block. + current->entry_speed_sqr = min( current->max_entry_speed_sqr, + MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED + 2*current->acceleration*current->millimeters); + calculate_trapezoid_for_block(current, current->entry_speed_sqr, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED); + + + // Reverse Pass: Back plan the deceleration curve from the last block in buffer. Cease + // planning when: (1) the last optimal planned pointer is reached. (2) the safe block + // pointer is reached, whereby the planned pointer is updated. + float entry_speed_sqr; + block_t *next; + block_index = prev_block_index(block_index); + while (block_index != block_buffer_planned) { + next = current; + current = &block_buffer[block_index]; + + // Exit loop and update planned pointer when the tail/safe block is reached. + if (block_index == block_buffer_safe) { + block_buffer_planned = block_buffer_safe; + break; + } + + // Crudely maximize deceleration curve from the end of the non-optimally planned buffer to + // the optimal plan pointer. Forward pass will adjust and finish optimizing the plan. + if (current->entry_speed_sqr != current->max_entry_speed_sqr) { + entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters; + if (entry_speed_sqr < current->max_entry_speed_sqr) { + current->entry_speed_sqr = entry_speed_sqr; + } else { + current->entry_speed_sqr = current->max_entry_speed_sqr; + } + } + block_index = prev_block_index(block_index); + } + + // Forward Pass: Forward plan the acceleration curve from the planned pointer onward. + // Also scans for optimal plan breakpoints and appropriately updates the planned pointer. + block_index = block_buffer_planned; // Begin at buffer planned pointer + next = &block_buffer[prev_block_index(block_buffer_planned)]; // Set up for while loop + while (block_index != next_buffer_head) { + current = next; + next = &block_buffer[block_index]; + + // Any acceleration detected in the forward pass automatically moves the optimal planned + // pointer forward, since everything before this is all optimal. In other words, nothing + // can improve the plan from the buffer tail to the planned pointer by logic. + if (current->entry_speed_sqr < next->entry_speed_sqr) { + block_buffer_planned = block_index; + entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters; + if (entry_speed_sqr < next->entry_speed_sqr) { + next->entry_speed_sqr = entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass set this. + } + } + + // Any block set at its maximum entry speed also creates an optimal plan up to this + // point in the buffer. The optimally planned pointer is updated. + if (next->entry_speed_sqr == next->max_entry_speed_sqr) { + block_buffer_planned = block_index; + } + + // Automatically recalculate trapezoid for all buffer blocks from last plan's optimal planned + // pointer to the end of the buffer, except the last block. + calculate_trapezoid_for_block(current, current->entry_speed_sqr, next->entry_speed_sqr); + block_index = next_block_index( block_index ); + } + + } + +} + + +void plan_init() +{ + block_buffer_tail = block_buffer_head; + next_buffer_head = next_block_index(block_buffer_head); + block_buffer_planned = block_buffer_head; + memset(&pl, 0, sizeof(pl)); // Clear planner struct +} + + +inline void plan_discard_current_block() +{ + if (block_buffer_head != block_buffer_tail) { + block_buffer_tail = next_block_index( block_buffer_tail ); + } +} + + +inline block_t *plan_get_current_block() +{ + if (block_buffer_head == block_buffer_tail) { return(NULL); } + return(&block_buffer[block_buffer_tail]); +} + + +// Returns the availability status of the block ring buffer. True, if full. +uint8_t plan_check_full_buffer() +{ + if (block_buffer_tail == next_buffer_head) { return(true); } + return(false); +} + + +// Block until all buffered steps are executed or in a cycle state. Works with feed hold +// during a synchronize call, if it should happen. Also, waits for clean cycle end. +void plan_synchronize() +{ + while (plan_get_current_block() || sys.state == STATE_CYCLE) { + protocol_execute_runtime(); // Check and execute run-time commands + if (sys.abort) { return; } // Check for system abort + } +} + + +// Add a new linear movement to the buffer. target[N_AXIS] is the signed, absolute target position +// in millimeters. 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. +// All position data passed to the planner must be in terms of machine position to keep the planner +// independent of any coordinate system changes and offsets, which are handled by the g-code parser. +// NOTE: Assumes buffer is available. Buffer checks are handled at a higher level by motion_control. +// In other words, the buffer head is never equal to the buffer tail. Also the feed rate input value +// is used in three ways: as a normal feed rate if invert_feed_rate is false, as inverse time if +// invert_feed_rate is true, or as seek/rapids rate if the feed_rate value is negative (and +// invert_feed_rate always false). +void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) +{ + // Prepare and initialize new block + block_t *block = &block_buffer[block_buffer_head]; + block->step_event_count = 0; + block->millimeters = 0; + block->direction_bits = 0; + block->acceleration = SOME_LARGE_VALUE; // Scaled down to maximum acceleration later + + // Compute and store initial move distance data. + int32_t target_steps[N_AXIS]; + float unit_vec[N_AXIS], delta_mm; + uint8_t idx; + for (idx=0; idxsteps[idx] = labs(target_steps[idx]-pl.position[idx]); + block->step_event_count = max(block->step_event_count, block->steps[idx]); + + // Compute individual axes distance for move and prep unit vector calculations. + delta_mm = target[idx] - pl.last_target[idx]; + unit_vec[idx] = delta_mm; // Store unit vector numerator. Denominator computed later. + + // Incrementally compute total move distance by Euclidean norm + block->millimeters += delta_mm*delta_mm; + + // Set direction bits. Bit enabled always means direction is negative. + if (delta_mm < 0 ) { block->direction_bits |= get_direction_mask(idx); } + } + block->millimeters = sqrt(block->millimeters); // Complete millimeters calculation + + // Bail if this is a zero-length block + if (block->step_event_count == 0) { return; } + + // Adjust feed_rate value to mm/min depending on type of rate input (normal, inverse time, or rapids) + // TODO: Need to distinguish a rapids vs feed move for overrides. Some flag of some sort. + if (feed_rate < 0) { feed_rate = SOME_LARGE_VALUE; } // Scaled down to absolute max/rapids rate later + else if (invert_feed_rate) { feed_rate = block->millimeters/feed_rate; } + + // Calculate the unit vector of the line move and the block maximum feed rate and acceleration limited + // by the maximum possible values. Block rapids rates are computed or feed rates are scaled down so + // they don't exceed the maximum axes velocities. The block acceleration is maximized based on direction + // and axes properties as well. + // NOTE: This calculation assumes all axes are orthogonal (Cartesian) and works with ABC-axes, + // if they are also orthogonal/independent. Operates on the absolute value of the unit vector. + float inverse_unit_vec_value; + float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple float divides + float junction_cos_theta = 0; + for (idx=0; idxacceleration = min(block->acceleration,settings.acceleration[idx]*inverse_unit_vec_value); + + // Incrementally compute cosine of angle between previous and current path. Cos(theta) of the junction + // between the current move and the previous move is simply the dot product of the two unit vectors, + // where prev_unit_vec is negative. Used later to compute maximum junction speed. + junction_cos_theta -= pl.previous_unit_vec[idx] * unit_vec[idx]; + } + } + + /* Compute maximum allowable entry speed at junction by centripetal acceleration approximation. + 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 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. This approach does not actually deviate + from path, but used as a robust way to compute cornering speeds, as it takes into account the + nonlinearities of both the junction angle and junction velocity. + NOTE: If the junction deviation value is finite, Grbl executes the motions in an exact path + mode (G61). If the junction deviation value is zero, Grbl will execute the motion in an exact + stop mode (G61.1) manner. In the future, if continuous mode (G64) is desired, the math here + is exactly the same. Instead of motioning all the way to junction point, the machine will + just follow the arc circle defined here. The Arduino doesn't have the CPU cycles to perform + a continuous mode path, but ARM-based microcontrollers most certainly do. + */ + // TODO: Acceleration need to be limited by the minimum of the two junctions. + // TODO: Need to setup a method to handle zero junction speeds when starting from rest. + if (block_buffer_head == block_buffer_tail) { + block->max_entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED; + } else { + // NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta). + float sin_theta_d2 = sqrt(0.5*(1.0-junction_cos_theta)); // Trig half angle identity. Always positive. + block->max_entry_speed_sqr = (block->acceleration * settings.junction_deviation * sin_theta_d2)/(1.0-sin_theta_d2); + } + + // Store block nominal speed and rate + block->nominal_speed_sqr = feed_rate*feed_rate; // (mm/min)^2. Always > 0 + block->nominal_rate = ceil(feed_rate*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + + // Compute and store acceleration and distance traveled per step event. + block->rate_delta = ceil(block->acceleration* + ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) + block->d_next = ceil((block->millimeters*INV_TIME_MULTIPLIER)/block->step_event_count); // (mult*mm/step) + + // Update previous path unit_vector and nominal speed (squared) + memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // pl.previous_unit_vec[] = unit_vec[] + pl.previous_nominal_speed_sqr = block->nominal_speed_sqr; + + // Update planner position + memcpy(pl.position, target_steps, sizeof(target_steps)); // pl.position[] = target_steps[] + memcpy(pl.last_target, target, sizeof(target)); // pl.last_target[] = target[] + + planner_recalculate(); + + // Update buffer head and next buffer head indices. + // NOTE: The buffer head update is atomic since it's one byte. Performed after the new plan + // calculations to help prevent overwriting scenarios with adding a new block to a low buffer. + block_buffer_head = next_buffer_head; + next_buffer_head = next_block_index(block_buffer_head); +} + + +// Reset the planner position vectors. Called by the system abort/initialization routine. +void plan_sync_position() +{ + uint8_t idx; + for (idx=0; idxmillimeters = (block->millimeters*step_events_remaining)/block->step_event_count; + block->step_event_count = step_events_remaining; + + // Re-plan from a complete stop. Reset planner entry speeds and buffer planned pointer. + block->entry_speed_sqr = 0.0; + block->max_entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED; + block_buffer_planned = block_buffer_tail; + planner_recalculate(); +} diff --git a/planner_old.h b/planner_old.h new file mode 100644 index 0000000..84ecc4b --- /dev/null +++ b/planner_old.h @@ -0,0 +1,83 @@ +/* + planner.h - buffers movement commands and manages the acceleration profile plan + Part of Grbl + + Copyright (c) 2011-2013 Sungeun K. Jeon + Copyright (c) 2009-2011 Simen Svale Skogsrud + + 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 + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#ifndef planner_h +#define planner_h +#include "nuts_bolts.h" + +// The number of linear motions that can be in the plan at any give time +#ifndef BLOCK_BUFFER_SIZE + #define BLOCK_BUFFER_SIZE 17 +#endif + +// This struct is used when buffering the setup for each linear movement "nominal" values are as specified in +// the source g-code and may never actually be reached if acceleration management is active. +typedef struct { + + // Fields used by the bresenham algorithm for tracing the line + uint8_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) + uint32_t steps[N_AXIS]; // Step count along each axis + int32_t step_event_count; // The number of step events required to complete this block + + // Fields used by the motion planner to manage acceleration + float nominal_speed_sqr; // Axis-limit adjusted nominal speed for this block in (mm/min)^2 + float entry_speed_sqr; // Entry speed at previous-current block junction in (mm/min)^2 + float max_entry_speed_sqr; // Maximum allowable junction entry speed in (mm/min)^2 + float millimeters; // The total travel of this block in mm + float acceleration; // Axes-limit adjusted line acceleration in mm/min^2 + + // Settings for the trapezoid generator + uint32_t initial_rate; // The step rate at start of block + int32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive) + uint32_t decelerate_after; // The index of the step event on which to start decelerating + uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute + uint32_t d_next; // Scaled distance to next step + +} block_t; + +// Initialize the motion plan subsystem +void plan_init(); + +// Add a new linear movement to the buffer. target[N_AXIS] is the signed, absolute target position +// in millimeters. 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(float *target, float 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. +void plan_discard_current_block(); + +// Gets the current block. Returns NULL if buffer empty +block_t *plan_get_current_block(); + +// Reset the planner position vector (in steps) +void plan_sync_position(); + +// Reinitialize plan with a partially completed block +void plan_cycle_reinitialize(int32_t step_events_remaining); + +// Returns the status of the block ring buffer. True, if buffer is full. +uint8_t plan_check_full_buffer(); + +// Block until all buffered steps are executed +void plan_synchronize(); + +#endif diff --git a/print.c b/print.c index 2f820d5..7a7686b 100644 --- a/print.c +++ b/print.c @@ -77,7 +77,7 @@ void print_uint8_base2(uint8_t n) serial_write('0' + buf[i - 1]); } -static void print_uint32_base10(unsigned long n) +void print_uint32_base10(unsigned long n) { unsigned char buf[10]; uint8_t i = 0; diff --git a/print.h b/print.h index 9983aee..8a161c1 100644 --- a/print.h +++ b/print.h @@ -31,6 +31,8 @@ void printPgmString(const char *s); void printInteger(long n); +void print_uint32_base10(uint32_t n); + void print_uint8_base2(uint8_t n); void printFloat(float n); diff --git a/protocol.c b/protocol.c index fd64c3d..014b607 100644 --- a/protocol.c +++ b/protocol.c @@ -3,7 +3,7 @@ Part of Grbl Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2011-2012 Sungeun K. Jeon + Copyright (c) 2011-2013 Sungeun K. 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 @@ -37,10 +37,16 @@ static uint8_t char_counter; // Last character counter in line variable. static uint8_t iscomment; // Comment/block delete flag for processor to ignore comment characters. +static void protocol_reset_line_buffer() +{ + char_counter = 0; + iscomment = false; +} + + void protocol_init() { - char_counter = 0; // Reset line input - iscomment = false; + protocol_reset_line_buffer(); // Reset line input report_init_message(); // Welcome message PINOUT_DDR &= ~(PINOUT_MASK); // Set as input pins @@ -49,6 +55,7 @@ void protocol_init() PCICR |= (1 << PINOUT_INT); // Enable Pin Change Interrupt } + // Executes user startup script, if stored. void protocol_execute_startup() { @@ -65,6 +72,7 @@ void protocol_execute_startup() } } + // Pin change interrupt for pin-out commands, i.e. cycle start, feed hold, and reset. Sets // only the runtime command execute variable to have the main program execute these when // its ready. This works exactly like the character-based runtime commands when picked off @@ -96,6 +104,9 @@ ISR(PINOUT_INT_vect) // limit switches, or the main program. void protocol_execute_runtime() { + // Reload step segment buffer + st_prep_buffer(); + if (sys.execute) { // Enter only if any bit flag is true uint8_t rt_exec = sys.execute; // Avoid calling volatile multiple times @@ -104,14 +115,18 @@ void protocol_execute_runtime() // loop until system reset/abort. if (rt_exec & (EXEC_ALARM | EXEC_CRIT_EVENT)) { sys.state = STATE_ALARM; // Set system alarm state - - if (rt_exec & EXEC_CRIT_EVENT) { + + // Critical event. Only hard/soft limit errors currently qualify. + if (rt_exec & EXEC_CRIT_EVENT) { + report_alarm_message(ALARM_LIMIT_ERROR); report_feedback_message(MESSAGE_CRITICAL_EVENT); bit_false(sys.execute,EXEC_RESET); // Disable any existing reset do { // Nothing. Block EVERYTHING until user issues reset or power cycles. Hard limits // typically occur while unattended or not paying attention. Gives the user time - // to do what is needed before resetting, like killing the incoming stream. + // to do what is needed before resetting, like killing the incoming stream. The + // same could be said about soft limits. While the position is not lost, the incoming + // stream could be still engaged and cause a serious crash if it continues afterwards. } while (bit_isfalse(sys.execute,EXEC_RESET)); // Standard alarm event. Only abort during motion qualifies. @@ -138,6 +153,8 @@ void protocol_execute_runtime() // Initiate stepper feed hold if (rt_exec & EXEC_FEED_HOLD) { + // !!! During a cycle, the segment buffer has just been reloaded and full. So the math involved + // with the feed hold should be fine for most, if not all, operational scenarios. st_feed_hold(); // Initiate feed hold. bit_false(sys.execute,EXEC_FEED_HOLD); } @@ -301,9 +318,8 @@ void protocol_process() // Empty or comment line. Skip block. report_status_message(STATUS_OK); // Send status message for syncing purposes. } - char_counter = 0; // Reset line buffer index - iscomment = false; // Reset comment flag - + protocol_reset_line_buffer(); + } else { if (iscomment) { // Throw away all comment characters @@ -320,7 +336,9 @@ void protocol_process() // Enable comments flag and ignore all characters until ')' or EOL. iscomment = true; } else if (char_counter >= LINE_BUFFER_SIZE-1) { - // Throw away any characters beyond the end of the line buffer + // Detect line buffer overflow. Report error and reset line buffer. + report_status_message(STATUS_OVERFLOW); + protocol_reset_line_buffer(); } else if (c >= 'a' && c <= 'z') { // Upcase lowercase line[char_counter++] = c-'a'+'A'; } else { diff --git a/protocol.h b/protocol.h index 209d19d..4d90c1c 100644 --- a/protocol.h +++ b/protocol.h @@ -30,7 +30,7 @@ // memory space we can invest into here or we re-write the g-code parser not to have his // buffer. #ifndef LINE_BUFFER_SIZE - #define LINE_BUFFER_SIZE 50 + #define LINE_BUFFER_SIZE 70 #endif // Initialize the serial protocol diff --git a/report.c b/report.c index 5b64c13..8688e4f 100644 --- a/report.c +++ b/report.c @@ -2,7 +2,7 @@ report.c - reporting and messaging methods Part of Grbl - Copyright (c) 2012 Sungeun K. Jeon + Copyright (c) 2012-2013 Sungeun K. 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 @@ -74,6 +74,10 @@ void report_status_message(uint8_t status_code) printPgmString(PSTR("Busy or queued")); break; case STATUS_ALARM_LOCK: printPgmString(PSTR("Alarm lock")); break; + case STATUS_SOFT_LIMIT_ERROR: + printPgmString(PSTR("Homing not enabled")); break; + case STATUS_OVERFLOW: + printPgmString(PSTR("Line overflow")); break; } printPgmString(PSTR("\r\n")); } @@ -84,8 +88,8 @@ void report_alarm_message(int8_t alarm_code) { printPgmString(PSTR("ALARM: ")); switch (alarm_code) { - case ALARM_HARD_LIMIT: - printPgmString(PSTR("Hard limit")); break; + case ALARM_LIMIT_ERROR: + printPgmString(PSTR("Hard/soft limit")); break; case ALARM_ABORT_CYCLE: printPgmString(PSTR("Abort during cycle")); break; case ALARM_SOFT_LIMIT: @@ -114,7 +118,7 @@ void report_feedback_message(uint8_t message_code) case MESSAGE_ENABLED: printPgmString(PSTR("Enabled")); break; case MESSAGE_DISABLED: - printPgmString(PSTR("Disabled")); break; + printPgmString(PSTR("Disabled")); break; } printPgmString(PSTR("]\r\n")); } @@ -155,30 +159,30 @@ void report_grbl_settings() { printPgmString(PSTR(" (z v_max, mm/min)\r\n$6=")); printFloat(settings.acceleration[X_AXIS]/(60*60)); // Convert from mm/min^2 for human readability printPgmString(PSTR(" (x accel, mm/sec^2)\r\n$7=")); printFloat(settings.acceleration[Y_AXIS]/(60*60)); // Convert from mm/min^2 for human readability printPgmString(PSTR(" (y accel, mm/sec^2)\r\n$8=")); printFloat(settings.acceleration[Z_AXIS]/(60*60)); // Convert from mm/min^2 for human readability - printPgmString(PSTR(" (z accel, mm/sec^2)\r\n$9=")); printInteger(settings.pulse_microseconds); - printPgmString(PSTR(" (step pulse, usec)\r\n$10=")); printFloat(settings.default_feed_rate); - printPgmString(PSTR(" (default feed, mm/min)\r\n$11=")); printInteger(settings.invert_mask); + printPgmString(PSTR(" (z accel, mm/sec^2)\r\n$9=")); printFloat(-settings.max_travel[X_AXIS]); // Grbl internally store this as negative. + printPgmString(PSTR(" (x max travel, mm)\r\n$10=")); printFloat(-settings.max_travel[Y_AXIS]); // Grbl internally store this as negative. + printPgmString(PSTR(" (y max travel, mm)\r\n$11=")); printFloat(-settings.max_travel[Z_AXIS]); // Grbl internally store this as negative. + printPgmString(PSTR(" (z max travel, mm)\r\n$12=")); printInteger(settings.pulse_microseconds); + printPgmString(PSTR(" (step pulse, usec)\r\n$13=")); printFloat(settings.default_feed_rate); + printPgmString(PSTR(" (default feed, mm/min)\r\n$14=")); printInteger(settings.invert_mask); printPgmString(PSTR(" (step port invert mask, int:")); print_uint8_base2(settings.invert_mask); - printPgmString(PSTR(")\r\n$12=")); printInteger(settings.stepper_idle_lock_time); - printPgmString(PSTR(" (step idle delay, msec)\r\n$13=")); printFloat(settings.junction_deviation); - printPgmString(PSTR(" (junction deviation, mm)\r\n$14=")); printFloat(settings.arc_tolerance); - printPgmString(PSTR(" (arc tolerance, mm)\r\n$15=")); printInteger(settings.decimal_places); - printPgmString(PSTR(" (n-decimals, int)\r\n$16=")); printInteger(bit_istrue(settings.flags,BITFLAG_REPORT_INCHES)); - printPgmString(PSTR(" (report inches, bool)\r\n$17=")); printInteger(bit_istrue(settings.flags,BITFLAG_AUTO_START)); - printPgmString(PSTR(" (auto start, bool)\r\n$18=")); printInteger(bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)); - printPgmString(PSTR(" (invert step enable, bool)\r\n$19=")); printInteger(bit_istrue(settings.flags,BITFLAG_HARD_LIMIT_ENABLE)); - printPgmString(PSTR(" (hard limits, bool)\r\n$20=")); printInteger(bit_istrue(settings.flags,BITFLAG_HOMING_ENABLE)); - printPgmString(PSTR(" (homing cycle, bool)\r\n$21=")); printInteger(settings.homing_dir_mask); + printPgmString(PSTR(")\r\n$15=")); printInteger(settings.stepper_idle_lock_time); + printPgmString(PSTR(" (step idle delay, msec)\r\n$16=")); printFloat(settings.junction_deviation); + printPgmString(PSTR(" (junction deviation, mm)\r\n$17=")); printFloat(settings.arc_tolerance); + printPgmString(PSTR(" (arc tolerance, mm)\r\n$18=")); printInteger(settings.decimal_places); + printPgmString(PSTR(" (n-decimals, int)\r\n$19=")); printInteger(bit_istrue(settings.flags,BITFLAG_REPORT_INCHES)); + printPgmString(PSTR(" (report inches, bool)\r\n$20=")); printInteger(bit_istrue(settings.flags,BITFLAG_AUTO_START)); + printPgmString(PSTR(" (auto start, bool)\r\n$21=")); printInteger(bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)); + printPgmString(PSTR(" (invert step enable, bool)\r\n$22=")); printInteger(bit_istrue(settings.flags,BITFLAG_SOFT_LIMIT_ENABLE)); + printPgmString(PSTR(" (soft limits, bool)\r\n$23=")); printInteger(bit_istrue(settings.flags,BITFLAG_HARD_LIMIT_ENABLE)); + printPgmString(PSTR(" (hard limits, bool)\r\n$24=")); printInteger(bit_istrue(settings.flags,BITFLAG_HOMING_ENABLE)); + printPgmString(PSTR(" (homing cycle, bool)\r\n$25=")); printInteger(settings.homing_dir_mask); printPgmString(PSTR(" (homing dir invert mask, int:")); print_uint8_base2(settings.homing_dir_mask); - printPgmString(PSTR(")\r\n$22=")); printFloat(settings.homing_feed_rate); - printPgmString(PSTR(" (homing feed, mm/min)\r\n$23=")); printFloat(settings.homing_seek_rate); - printPgmString(PSTR(" (homing seek, mm/min)\r\n$24=")); printInteger(settings.homing_debounce_delay); - printPgmString(PSTR(" (homing debounce, msec)\r\n$25=")); printFloat(settings.homing_pulloff); - printPgmString(PSTR(" (homing pull-off, mm)\r\n$26=")); printFloat(settings.max_travel[X_AXIS]); - printPgmString(PSTR(" (x travel, mm)\r\n$27=")); printFloat(settings.max_travel[Y_AXIS]); - printPgmString(PSTR(" (y travel, mm)\r\n$28=")); printFloat(settings.max_travel[Z_AXIS]); - printPgmString(PSTR(" (z travel, mm)\r\n$29=")); printInteger(bit_istrue(settings.flags,BITFLAG_SOFT_LIMIT_ENABLE)); - printPgmString(PSTR(" (soft limits, bool)\r\n")); + printPgmString(PSTR(")\r\n$26=")); printFloat(settings.homing_feed_rate); + printPgmString(PSTR(" (homing feed, mm/min)\r\n$27=")); printFloat(settings.homing_seek_rate); + printPgmString(PSTR(" (homing seek, mm/min)\r\n$28=")); printInteger(settings.homing_debounce_delay); + printPgmString(PSTR(" (homing debounce, msec)\r\n$29=")); printFloat(settings.homing_pulloff); + printPgmString(PSTR(" (homing pull-off, mm)\r\n")); } diff --git a/report.h b/report.h index cd3cb2b..26af23d 100644 --- a/report.h +++ b/report.h @@ -2,7 +2,7 @@ report.h - reporting and messaging methods Part of Grbl - Copyright (c) 2012 Sungeun K. Jeon + Copyright (c) 2012-2013 Sungeun K. 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 +35,11 @@ #define STATUS_SETTING_READ_FAIL 10 #define STATUS_IDLE_ERROR 11 #define STATUS_ALARM_LOCK 12 +#define STATUS_SOFT_LIMIT_ERROR 13 +#define STATUS_OVERFLOW 14 // Define Grbl alarm codes. Less than zero to distinguish alarm error from status error. -#define ALARM_HARD_LIMIT -1 +#define ALARM_LIMIT_ERROR -1 #define ALARM_ABORT_CYCLE -2 #define ALARM_SOFT_LIMIT -3 diff --git a/serial.c b/serial.c index 69fa717..d3325c7 100644 --- a/serial.c +++ b/serial.c @@ -91,11 +91,7 @@ void serial_write(uint8_t data) { } // Data Register Empty Interrupt handler -#ifdef __AVR_ATmega644P__ -ISR(USART0_UDRE_vect) -#else -ISR(USART_UDRE_vect) -#endif +ISR(SERIAL_UDRE) { // Temporary tx_buffer_tail (to optimize for volatile) uint8_t tail = tx_buffer_tail; @@ -144,11 +140,7 @@ uint8_t serial_read() } } -#ifdef __AVR_ATmega644P__ -ISR(USART0_RX_vect) -#else -ISR(USART_RX_vect) -#endif +ISR(SERIAL_RX) { uint8_t data = UDR0; uint8_t next_head; diff --git a/settings.c b/settings.c index f1453f7..8f628a5 100644 --- a/settings.c +++ b/settings.c @@ -3,7 +3,7 @@ Part of Grbl Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2011-2012 Sungeun K. Jeon + Copyright (c) 2011-2013 Sungeun K. 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 @@ -88,6 +88,7 @@ void settings_reset(bool reset_all) { if (DEFAULT_REPORT_INCHES) { settings.flags |= BITFLAG_REPORT_INCHES; } if (DEFAULT_AUTO_START) { settings.flags |= BITFLAG_AUTO_START; } if (DEFAULT_INVERT_ST_ENABLE) { settings.flags |= BITFLAG_INVERT_ST_ENABLE; } + if (DEFAULT_SOFT_LIMIT_ENABLE) { settings.flags |= BITFLAG_SOFT_LIMIT_ENABLE; } if (DEFAULT_HARD_LIMIT_ENABLE) { settings.flags |= BITFLAG_HARD_LIMIT_ENABLE; } if (DEFAULT_HOMING_ENABLE) { settings.flags |= BITFLAG_HOMING_ENABLE; } settings.homing_dir_mask = DEFAULT_HOMING_DIR_MASK; @@ -97,6 +98,9 @@ void settings_reset(bool reset_all) { settings.homing_pulloff = DEFAULT_HOMING_PULLOFF; settings.stepper_idle_lock_time = DEFAULT_STEPPER_IDLE_LOCK_TIME; settings.decimal_places = DEFAULT_DECIMAL_PLACES; + settings.max_travel[X_AXIS] = DEFAULT_X_MAX_TRAVEL; + settings.max_travel[Y_AXIS] = DEFAULT_Y_MAX_TRAVEL; + settings.max_travel[Z_AXIS] = DEFAULT_Z_MAX_TRAVEL; write_global_settings(); } @@ -165,48 +169,53 @@ uint8_t settings_store_global_setting(int parameter, float value) { case 6: settings.acceleration[X_AXIS] = value*60*60; break; // Convert to mm/min^2 for grbl internal use. case 7: settings.acceleration[Y_AXIS] = value*60*60; break; // Convert to mm/min^2 for grbl internal use. case 8: settings.acceleration[Z_AXIS] = value*60*60; break; // Convert to mm/min^2 for grbl internal use. - case 9: + case 9: settings.max_travel[X_AXIS] = -value; break; // Store as negative for grbl internal use. + case 10: settings.max_travel[Y_AXIS] = -value; break; // Store as negative for grbl internal use. + case 11: settings.max_travel[Z_AXIS] = -value; break; // Store as negative for grbl internal use. + case 12: if (value < 3) { return(STATUS_SETTING_STEP_PULSE_MIN); } settings.pulse_microseconds = round(value); break; - case 10: settings.default_feed_rate = value; break; - case 11: settings.invert_mask = trunc(value); break; - case 12: settings.stepper_idle_lock_time = round(value); break; - case 13: settings.junction_deviation = fabs(value); break; - case 14: settings.arc_tolerance = value; break; - case 15: settings.decimal_places = round(value); break; - case 16: + case 13: settings.default_feed_rate = value; break; + case 14: settings.invert_mask = trunc(value); break; + case 15: settings.stepper_idle_lock_time = round(value); break; + case 16: settings.junction_deviation = fabs(value); break; + case 17: settings.arc_tolerance = value; break; + case 18: settings.decimal_places = round(value); break; + case 19: if (value) { settings.flags |= BITFLAG_REPORT_INCHES; } else { settings.flags &= ~BITFLAG_REPORT_INCHES; } break; - case 17: // Reset to ensure change. Immediate re-init may cause problems. + case 20: // Reset to ensure change. Immediate re-init may cause problems. if (value) { settings.flags |= BITFLAG_AUTO_START; } else { settings.flags &= ~BITFLAG_AUTO_START; } break; - case 18: // Reset to ensure change. Immediate re-init may cause problems. + case 21: // Reset to ensure change. Immediate re-init may cause problems. if (value) { settings.flags |= BITFLAG_INVERT_ST_ENABLE; } else { settings.flags &= ~BITFLAG_INVERT_ST_ENABLE; } break; - case 19: + case 22: + if (value) { + if (bit_isfalse(settings.flags, BITFLAG_HOMING_ENABLE)) { return(STATUS_SOFT_LIMIT_ERROR); } + settings.flags |= BITFLAG_SOFT_LIMIT_ENABLE; + } else { settings.flags &= ~BITFLAG_SOFT_LIMIT_ENABLE; } + break; + case 23: if (value) { settings.flags |= BITFLAG_HARD_LIMIT_ENABLE; } else { settings.flags &= ~BITFLAG_HARD_LIMIT_ENABLE; } limits_init(); // Re-init to immediately change. NOTE: Nice to have but could be problematic later. break; - case 20: + case 24: if (value) { settings.flags |= BITFLAG_HOMING_ENABLE; } - else { settings.flags &= ~BITFLAG_HOMING_ENABLE; } - break; - case 21: settings.homing_dir_mask = trunc(value); break; - case 22: settings.homing_feed_rate = value; break; - case 23: settings.homing_seek_rate = value; break; - case 24: settings.homing_debounce_delay = round(value); break; - case 25: settings.homing_pulloff = value; break; - case 26: case 27: case 28: - if (value <= 0.0) { return(STATUS_SETTING_VALUE_NEG); } - settings.max_travel[parameter-26] = value; break; - case 29: - if (value) { settings.flags |= BITFLAG_SOFT_LIMIT_ENABLE; } - else { settings.flags &= ~BITFLAG_SOFT_LIMIT_ENABLE; } + else { + settings.flags &= ~BITFLAG_HOMING_ENABLE; + settings.flags &= ~BITFLAG_SOFT_LIMIT_ENABLE; + } break; + case 25: settings.homing_dir_mask = trunc(value); break; + case 26: settings.homing_feed_rate = value; break; + case 27: settings.homing_seek_rate = value; break; + case 28: settings.homing_debounce_delay = round(value); break; + case 29: settings.homing_pulloff = value; break; default: return(STATUS_INVALID_STATEMENT); } diff --git a/settings.h b/settings.h index 2525bd3..3369845 100644 --- a/settings.h +++ b/settings.h @@ -3,7 +3,7 @@ Part of Grbl Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2011-2012 Sungeun K. Jeon + Copyright (c) 2011-2013 Sungeun K. 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 @@ -29,15 +29,15 @@ // Version of the EEPROM data. Will be used to migrate existing data from older versions of Grbl // when firmware is upgraded. Always stored in byte 0 of eeprom -#define SETTINGS_VERSION 52 +#define SETTINGS_VERSION 53 // Define bit flag masks for the boolean settings in settings.flag. #define BITFLAG_REPORT_INCHES bit(0) #define BITFLAG_AUTO_START bit(1) #define BITFLAG_INVERT_ST_ENABLE bit(2) #define BITFLAG_HARD_LIMIT_ENABLE bit(3) -#define BITFLAG_SOFT_LIMIT_ENABLE bit(4) -#define BITFLAG_HOMING_ENABLE bit(5) +#define BITFLAG_HOMING_ENABLE bit(4) +#define BITFLAG_SOFT_LIMIT_ENABLE bit(5) // Define EEPROM memory address location values for Grbl settings and parameters // NOTE: The Atmega328p has 1KB EEPROM. The upper half is reserved for parameters and diff --git a/stepper.c b/stepper.c index 8c066af..a70a744 100644 --- a/stepper.c +++ b/stepper.c @@ -2,7 +2,7 @@ stepper.c - stepper motor driver: executes motion plans using stepper motors Part of Grbl - Copyright (c) 2011-2012 Sungeun K. Jeon + Copyright (c) 2011-2013 Sungeun K. Jeon Copyright (c) 2009-2011 Simen Svale Skogsrud Grbl is free software: you can redistribute it and/or modify @@ -19,20 +19,32 @@ along with Grbl. If not, see . */ -/* The timer calculations of this module informed by the 'RepRap cartesian firmware' by Zack Smith - and Philipp Tiefenbacher. */ - #include #include "stepper.h" #include "config.h" #include "settings.h" #include "planner.h" +#include "nuts_bolts.h" // Some useful constants #define TICKS_PER_MICROSECOND (F_CPU/1000000) -#define CRUISE_RAMP 0 -#define ACCEL_RAMP 1 -#define DECEL_RAMP 2 + +#define RAMP_NOOP_CRUISE 0 +#define RAMP_ACCEL 1 +#define RAMP_DECEL 2 + +#define LOAD_NOOP 0 +#define LOAD_SEGMENT 1 +#define LOAD_BLOCK 2 + +#define SEGMENT_NOOP 0 +#define SEGMENT_END_OF_BLOCK bit(0) +#define RAMP_CHANGE_ACCEL bit(1) +#define RAMP_CHANGE_DECEL bit(2) + +#define MINIMUM_STEPS_PER_SEGMENT 1 // Don't change + +#define SEGMENT_BUFFER_SIZE 6 // Stepper state variable. Contains running data and trapezoid variables. typedef struct { @@ -40,44 +52,85 @@ typedef struct { int32_t counter_x, // Counter variables for the bresenham line tracer counter_y, counter_z; - uint32_t event_count; // Total event count. Retained for feed holds. - uint32_t step_events_remaining; // Steps remaining in motion + uint8_t segment_steps_remaining; // Steps remaining in line segment motion - // Used by Pramod Ranade inverse time algorithm - int32_t delta_d; // Ranade distance traveled per interrupt tick - int32_t d_counter; // Ranade distance traveled since last step event - uint8_t ramp_count; // Acceleration interrupt tick counter. - uint8_t ramp_type; // Ramp type variable. - uint8_t execute_step; // Flags step execution for each interrupt. + // Used by inverse time algorithm to track step rate + int32_t counter_dist; // Inverse time distance traveled since last step event + uint32_t ramp_rate; // Inverse time distance traveled per interrupt tick + uint32_t dist_per_tick; + // Used by the stepper driver interrupt + uint8_t execute_step; // Flags step execution for each interrupt. + uint8_t step_pulse_time; // Step pulse reset time after step rise + uint8_t out_bits; // The next stepping-bits to be output + uint8_t load_flag; + + uint8_t counter_ramp; + uint8_t ramp_type; } stepper_t; static stepper_t st; -static block_t *current_block; // A pointer to the block currently being traced -// Used by the stepper driver interrupt -static uint8_t step_pulse_time; // Step pulse reset time after step rise -static uint8_t out_bits; // The next stepping-bits to be output +// Stores stepper common data for executing steps in the segment buffer. Data can change mid-block when the +// planner updates the remaining block velocity profile with a more optimal plan or a feedrate override occurs. +// NOTE: Normally, this buffer is partially in-use, but, for the worst case scenario, it will never exceed +// the number of accessible stepper buffer segments (SEGMENT_BUFFER_SIZE-1). +typedef struct { + int32_t step_events_remaining; // Tracks step event count for the executing planner block + uint32_t dist_per_step; // Scaled distance to next step + uint32_t initial_rate; // Initialized step rate at re/start of a planner block + uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute + uint32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive) + uint32_t current_approx_rate; // Tracks the approximate segment rate to predict steps per segment to execute + int32_t decelerate_after; // Tracks when to initiate deceleration according to the planner block + float mm_per_step; +} st_data_t; +static st_data_t segment_data[SEGMENT_BUFFER_SIZE-1]; -// NOTE: If the main interrupt is guaranteed to be complete before the next interrupt, then -// this blocking variable is no longer needed. Only here for safety reasons. -static volatile uint8_t busy; // True when "Stepper Driver Interrupt" is being serviced. Used to avoid retriggering that handler. +// Primary stepper segment ring buffer. Contains small, short line segments for the stepper algorithm to execute, +// which are "checked-out" incrementally from the first block in the planner buffer. Once "checked-out", the steps +// in the segments buffer cannot be modified by the planner, where the remaining planner block steps still can. +typedef struct { + uint8_t n_step; // Number of step events to be executed for this segment + uint8_t st_data_index; // Stepper buffer common data index. Uses this information to execute this segment. + uint8_t flag; // Stepper algorithm bit-flag for special execution conditions. +} st_segment_t; +static st_segment_t segment_buffer[SEGMENT_BUFFER_SIZE]; + +// Step segment ring buffer indices +static volatile uint8_t segment_buffer_tail; +static volatile uint8_t segment_buffer_head; +static uint8_t segment_next_head; + +static volatile uint8_t busy; // Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though. +static plan_block_t *pl_current_block; // A pointer to the planner block currently being traced +static st_segment_t *st_current_segment; +static st_data_t *st_current_data; + +// Pointers for the step segment being prepped from the planner buffer. Accessed only by the +// main program. Pointers may be planning segments or planner blocks ahead of what being executed. +static plan_block_t *pl_prep_block; // Pointer to the planner block being prepped +static st_data_t *st_prep_data; // Pointer to the stepper common data being prepped +static uint8_t pl_prep_index; // Index of planner block being prepped +static uint8_t st_data_prep_index; // Index of stepper common data block being prepped +static uint8_t pl_partial_block_flag; // Flag indicating the planner has modified the prepped planner block -// __________________________ -// /| |\ _________________ ^ -// / | | \ /| |\ | -// / | | \ / | | \ s -// / | | | | | \ p -// / | | | | | \ e -// +-----+------------------------+---+--+---------------+----+ e -// | BLOCK 1 | BLOCK 2 | d -// -// time -----> -// -// The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta -// until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after -// after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as -// +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. +/* __________________________ + /| |\ _________________ ^ + / | | \ /| |\ | + / | | \ / | | \ s + / | | | | | \ p + / | | | | | \ e + +-----+------------------------+---+--+---------------+----+ e + | BLOCK 1 | BLOCK 2 | d + + time -----> + + The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta + until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after + after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as + +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. +*/ // Stepper state initialization. Cycle should only start if the st.cycle_start flag is // enabled. Startup init and limits call this function but shouldn't start the cycle. @@ -91,23 +144,27 @@ void st_wake_up() } if (sys.state == STATE_CYCLE) { // Initialize stepper output bits - out_bits = settings.invert_mask; + st.out_bits = settings.invert_mask; // Initialize step pulse timing from settings. - step_pulse_time = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3); + st.step_pulse_time = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3); // Enable stepper driver interrupt st.execute_step = false; + st.load_flag = LOAD_BLOCK; + TCNT2 = 0; // Clear Timer2 TIMSK2 |= (1<direction_bits ^ settings.invert_mask; - st.execute_step = true; // Set flag to set direction bits. + sei(); // Re-enable interrupts to allow Stepper Port Reset Interrupt to fire on-time. + // NOTE: The remaining code in this ISR will finish before returning to main program. + + // If there is no step segment, attempt to pop one from the stepper buffer + if (st.load_flag != LOAD_NOOP) { + + // Anything in the buffer? If so, load and initialize next step segment. + if (segment_buffer_head != segment_buffer_tail) { - // Initialize Bresenham variables - st.counter_x = (current_block->step_event_count >> 1); - st.counter_y = st.counter_x; - st.counter_z = st.counter_x; - st.event_count = current_block->step_event_count; - st.step_events_remaining = st.event_count; - - // During feed hold, do not update Ranade counter, rate, or ramp type. Keep decelerating. - if (sys.state == STATE_CYCLE) { - // Initialize Ranade variables - st.d_counter = current_block->d_next; - st.delta_d = current_block->initial_rate; - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; + // Initialize new step segment and load number of steps to execute + st_current_segment = &segment_buffer[segment_buffer_tail]; + st.segment_steps_remaining = st_current_segment->n_step; + + // If the new segment starts a new planner block, initialize stepper variables and counters. + // NOTE: For new segments only, the step counters are not updated to ensure step phasing is continuous. + if (st.load_flag == LOAD_BLOCK) { + pl_current_block = plan_get_current_block(); // Should always be there. Stepper buffer handles this. + st_current_data = &segment_data[segment_buffer[segment_buffer_tail].st_data_index]; - // Initialize ramp type. - if (st.step_events_remaining == current_block->decelerate_after) { st.ramp_type = DECEL_RAMP; } - else if (st.delta_d == current_block->nominal_rate) { st.ramp_type = CRUISE_RAMP; } - else { st.ramp_type = ACCEL_RAMP; } + // Initialize direction bits for block. Set execute flag to set directions bits upon next ISR tick. + st.out_bits = pl_current_block->direction_bits ^ settings.invert_mask; + st.execute_step = true; + + // Initialize Bresenham line counters + st.counter_x = (pl_current_block->step_event_count >> 1); + st.counter_y = st.counter_x; + st.counter_z = st.counter_x; + + // Initialize inverse time, step rate data, and acceleration ramp counters + st.counter_dist = st_current_data->dist_per_step; // dist_per_step always greater than ramp_rate. + st.ramp_rate = st_current_data->initial_rate; + st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK/2; // Initialize ramp counter via midpoint rule + st.ramp_type = RAMP_NOOP_CRUISE; // Initialize as no ramp operation. Corrected later if necessary. + + // Ensure the initial step rate exceeds the MINIMUM_STEP_RATE. + if (st.ramp_rate < MINIMUM_STEP_RATE) { st.dist_per_tick = MINIMUM_STEP_RATE; } + else { st.dist_per_tick = st.ramp_rate; } + } + + // Check if ramp conditions have changed. If so, update ramp counters and control variables. + if ( st_current_segment->flag & (RAMP_CHANGE_DECEL | RAMP_CHANGE_ACCEL) ) { + /* Compute correct ramp count for a ramp change. Upon a switch from acceleration to deceleration, + or vice-versa, the new ramp count must be set to trigger the next acceleration tick equal to + the number of ramp ISR ticks counted since the last acceleration tick. This is ensures the + ramp is executed exactly as the plan dictates. Otherwise, when a ramp begins from a known + rate (nominal/cruise or initial), the ramp count must be set to ISR_TICKS_PER_ACCELERATION_TICK/2 + as mandated by the mid-point rule. For the latter conditions, the ramp count have been + initialized such that the following computation is still correct. */ + st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK-st.counter_ramp; + if ( st_current_segment->flag & RAMP_CHANGE_DECEL ) { st.ramp_type = RAMP_DECEL; } + else { st.ramp_type = RAMP_ACCEL; } } + st.load_flag = LOAD_NOOP; // Segment motion loaded. Set no-operation flag to skip during execution. + } else { + // Can't discard planner block here if a feed hold stops in middle of block. st_go_idle(); bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end - busy = false; return; // Nothing to do but exit. } + } // Adjust inverse time counter for ac/de-celerations - if (st.ramp_type) { - // Tick acceleration ramp counter - st.ramp_count--; - if (st.ramp_count == 0) { - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter - if (st.ramp_type == ACCEL_RAMP) { // Adjust velocity for acceleration - st.delta_d += current_block->rate_delta; - if (st.delta_d >= current_block->nominal_rate) { // Reached cruise state. - st.ramp_type = CRUISE_RAMP; - st.delta_d = current_block->nominal_rate; // Set cruise velocity + if (st.ramp_type) { // Ignored when ramp type is RAMP_NOOP_CRUISE + st.counter_ramp--; // Tick acceleration ramp counter + if (st.counter_ramp == 0) { // Adjust step rate when its time + st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter + if (st.ramp_type == RAMP_ACCEL) { // Adjust velocity for acceleration + st.ramp_rate += st_current_data->rate_delta; + if (st.ramp_rate >= st_current_data->nominal_rate) { // Reached nominal rate. + st.ramp_rate = st_current_data->nominal_rate; // Set cruising velocity + st.ramp_type = RAMP_NOOP_CRUISE; // Set ramp flag to cruising + st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK/2; // Re-initialize counter for next ramp change. } - } else if (st.ramp_type == DECEL_RAMP) { // Adjust velocity for deceleration - if (st.delta_d > current_block->rate_delta) { - st.delta_d -= current_block->rate_delta; - } else { - st.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow. + } else { // Adjust velocity for deceleration. + if (st.ramp_rate > st_current_data->rate_delta) { + st.ramp_rate -= st_current_data->rate_delta; + } else { // Moving near zero feed rate. Gracefully slow down. + st.ramp_rate >>= 1; // Integer divide by 2 until complete. Also prevents overflow. } } + // Adjust for minimum step rate, but retain operating ramp rate for accurate velocity tracing. + if (st.ramp_rate < MINIMUM_STEP_RATE) { st.dist_per_tick = MINIMUM_STEP_RATE; } + else { st.dist_per_tick = st.ramp_rate; } } } - - // Iterate Pramod Ranade inverse time counter. Triggers each Bresenham step event. - if (st.delta_d < MINIMUM_STEP_RATE) { st.d_counter -= MINIMUM_STEP_RATE; } - else { st.d_counter -= st.delta_d; } + + // Iterate inverse time counter. Triggers each Bresenham step event. + st.counter_dist -= st.dist_per_tick; // Execute Bresenham step event, when it's time to do so. - if (st.d_counter < 0) { - st.d_counter += current_block->d_next; - - // Check for feed hold state and execute accordingly. - if (sys.state == STATE_HOLD) { - if (st.ramp_type != DECEL_RAMP) { - st.ramp_type = DECEL_RAMP; - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; - } - if (st.delta_d <= current_block->rate_delta) { - st_go_idle(); - bit_true(sys.execute,EXEC_CYCLE_STOP); - busy = false; - return; - } - } - - // TODO: Vary Bresenham resolution for smoother motions or enable faster step rates (>20kHz). - - out_bits = current_block->direction_bits; // Reset out_bits and reload direction bits + if (st.counter_dist < 0) { + st.counter_dist += st_current_data->dist_per_step; // Reload inverse time counter + + st.out_bits = pl_current_block->direction_bits; // Reset out_bits and reload direction bits st.execute_step = true; // Execute step displacement profile by Bresenham line algorithm - st.counter_x -= current_block->steps_x; + st.counter_x -= pl_current_block->steps[X_AXIS]; if (st.counter_x < 0) { - out_bits |= (1<step_event_count; + if (st.out_bits & (1<steps_y; + st.counter_y -= pl_current_block->steps[Y_AXIS]; if (st.counter_y < 0) { - out_bits |= (1<step_event_count; + if (st.out_bits & (1<steps_z; + st.counter_z -= pl_current_block->steps[Z_AXIS]; if (st.counter_z < 0) { - out_bits |= (1<step_event_count; + if (st.out_bits & (1<decelerate_after) { - st.ramp_type = DECEL_RAMP; - if (st.step_events_remaining == current_block->decelerate_after) { - if (st.delta_d == current_block->nominal_rate) { - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid - } else { - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; // Set ramp counter for triangle - } - } - } + st.segment_steps_remaining--; // Decrement step events count + if (st.segment_steps_remaining == 0) { + // Line move is complete, set load line flag to check for new move. + // Check if last line move in planner block. Discard if so. + if (st_current_segment->flag & SEGMENT_END_OF_BLOCK) { + plan_discard_current_block(); + st.load_flag = LOAD_BLOCK; + } else { + st.load_flag = LOAD_SEGMENT; } - } else { - // If current block is finished, reset pointer - current_block = NULL; - plan_discard_current_block(); + + // Discard current segment by advancing buffer tail index + if ( ++segment_buffer_tail == SEGMENT_BUFFER_SIZE) { segment_buffer_tail = 0; } } - out_bits ^= settings.invert_mask; // Apply step port invert mask + st.out_bits ^= settings.invert_mask; // Apply step port invert mask } busy = false; // SPINDLE_ENABLE_PORT ^= 1<step_events_remaining); +// st.ramp_type = RAMP_ACCEL; +// st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK/2; +// st.ramp_rate = 0; +// sys.state = STATE_QUEUED; +// } else { +// sys.state = STATE_IDLE; +// } sys.state = STATE_IDLE; + +} + + +/* Prepares step segment buffer. Continuously called from main program. + + The segment buffer is an intermediary buffer interface between the execution of steps + by the stepper algorithm and the velocity profiles generated by the planner. The stepper + algorithm only executes steps within the segment buffer and is filled by the main program + when steps are "checked-out" from the first block in the planner buffer. This keeps the + step execution and planning optimization processes atomic and protected from each other. + The number of steps "checked-out" from the planner buffer and the number of segments in + the segment buffer is sized and computed such that no operation in the main program takes + longer than the time it takes the stepper algorithm to empty it before refilling it. + Currently, the segment buffer conservatively holds roughly up to 40-60 msec of steps. + + NOTE: The segment buffer executes a set number of steps over an approximate time period. + If we try to execute over a fixed time period, it is difficult to guarantee or predict + how many steps will execute over it, especially when the step pulse phasing between the + neighboring segments must also be kept consistent. Meaning that, if the last segment step + pulses right before a segment end, the next segment must delay its first pulse so that the + step pulses are consistently spaced apart over time to keep the step pulse train nice and + smooth. Keeping track of phasing and ensuring that the exact number of steps are executed + as defined by the planner block, the related computational overhead can get quickly and + prohibitively expensive, especially in real-time. + Since the stepper algorithm automatically takes care of the step pulse phasing with + its ramp and inverse time counters by retaining the count remainders, we don't have to + explicitly and expensively track and synchronize the exact number of steps, time, and + phasing of steps. All we need to do is approximate the number of steps in each segment + such that the segment buffer has enough execution time for the main program to do what + it needs to do and refill it when it comes back. In other words, we just need to compute + a cheap approximation of the current velocity and the number of steps over it. +*/ + +/* + TODO: Figure out how to enforce a deceleration when a feedrate override is reduced. + The problem is that when an override is reduced, the planner may not plan back to + the current rate. Meaning that the velocity profiles for certain conditions no longer + are trapezoidal or triangular. For example, if the current block is cruising at a + nominal rate and the feedrate override is reduced, the new nominal rate will now be + lower. The velocity profile must first decelerate to the new nominal rate and then + follow on the new plan. So the remaining velocity profile will have a decelerate, + cruise, and another decelerate. + Another issue is whether or not a feedrate override reduction causes a deceleration + that acts over several planner blocks. For example, say that the plan is already + heavily decelerating throughout it, reducing the feedrate will not do much to it. So, + how do we determine when to resume the new plan? How many blocks do we have to wait + until the new plan intersects with the deceleration curve? One plus though, the + deceleration will never be more than the number of blocks in the entire planner buffer, + but it theoretically can be equal to it when all planner blocks are decelerating already. +*/ +void st_prep_buffer() +{ + if (sys.state == STATE_QUEUED) { return; } // Block until a motion state is issued + while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer. + + // Initialize new segment + st_segment_t *prep_segment = &segment_buffer[segment_buffer_head]; + prep_segment->flag = SEGMENT_NOOP; + + // Determine if we need to load a new planner block. + if (pl_prep_block == NULL) { + pl_prep_block = plan_get_block_by_index(pl_prep_index); // Query planner for a queued block + if (pl_prep_block == NULL) { return; } // No planner blocks. Exit. + + // Increment stepper common data index + if ( ++st_data_prep_index == (SEGMENT_BUFFER_SIZE-1) ) { st_data_prep_index = 0; } + + // Check if the planner has re-computed this block mid-execution. If so, push the previous segment + // data. Otherwise, prepare a new segment data for the new planner block. + if (pl_partial_block_flag) { + + // Prepare new shared segment block data and copy the relevant last segment block data. + st_data_t *last_st_prep_data; + last_st_prep_data = st_prep_data; + st_prep_data = &segment_data[st_data_prep_index]; + + st_prep_data->step_events_remaining = last_st_prep_data->step_events_remaining; + st_prep_data->rate_delta = last_st_prep_data->rate_delta; + st_prep_data->dist_per_step = last_st_prep_data->dist_per_step; + st_prep_data->nominal_rate = last_st_prep_data->nominal_rate; // TODO: Feedrate overrides recomputes this. + + st_prep_data->mm_per_step = last_st_prep_data->mm_per_step; + + pl_partial_block_flag = false; // Reset flag + + } else { + + // Prepare commonly shared planner block data for the ensuing segment buffer moves ad-hoc, since + // the planner buffer can dynamically change the velocity profile data as blocks are added. + st_prep_data = &segment_data[st_data_prep_index]; + + // Initialize Bresenham variables + st_prep_data->step_events_remaining = pl_prep_block->step_event_count; + + // Convert planner block velocity profile data to stepper rate and step distance data. + st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + st_prep_data->rate_delta = ceil(pl_prep_block->acceleration* + ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) + st_prep_data->dist_per_step = ceil((pl_prep_block->millimeters*INV_TIME_MULTIPLIER)/pl_prep_block->step_event_count); // (mult*mm/step) + + // TODO: Check if we really need to store this. + st_prep_data->mm_per_step = pl_prep_block->millimeters/pl_prep_block->step_event_count; + + } + + // Convert planner entry speed to stepper initial rate. + st_prep_data->initial_rate = ceil(sqrt(pl_prep_block->entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + + // TODO: Nominal rate changes with feedrate override. + // st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + + st_prep_data->current_approx_rate = st_prep_data->initial_rate; + + // Calculate the planner block velocity profile type, determine deceleration point, and initial ramp. + float mm_decelerate_after = plan_calculate_velocity_profile(pl_prep_index); + st_prep_data->decelerate_after = ceil( mm_decelerate_after/st_prep_data->mm_per_step ); + if (st_prep_data->decelerate_after > 0) { // If 0, RAMP_CHANGE_DECEL flag is set later. + if (st_prep_data->initial_rate != st_prep_data->nominal_rate) { prep_segment->flag = RAMP_CHANGE_ACCEL; } + } + } + + // Set new segment to point to the current segment data block. + prep_segment->st_data_index = st_data_prep_index; + + // Approximate the velocity over the new segment using the already computed rate values. + // NOTE: This assumes that each segment will have an execution time roughly equal to every ACCELERATION_TICK. + // We do this to minimize memory and computational requirements. However, this could easily be replaced with + // a more exact approximation or have an user-defined time per segment, if CPU and memory overhead allows. + if (st_prep_data->decelerate_after <= 0) { + if (st_prep_data->decelerate_after == 0) { prep_segment->flag = RAMP_CHANGE_DECEL; } // Set segment deceleration flag + else { st_prep_data->current_approx_rate -= st_prep_data->rate_delta; } + if (st_prep_data->current_approx_rate < st_prep_data->rate_delta) { st_prep_data->current_approx_rate >>= 1; } + } else { + if (st_prep_data->current_approx_rate < st_prep_data->nominal_rate) { + st_prep_data->current_approx_rate += st_prep_data->rate_delta; + if (st_prep_data->current_approx_rate > st_prep_data->nominal_rate) { + st_prep_data->current_approx_rate = st_prep_data->nominal_rate; + } + } + } + + // TODO: Look into replacing the following dist_per_step divide with multiplying its inverse to save cycles. + + // Compute the number of steps in the prepped segment based on the approximate current rate. + // NOTE: The dist_per_step divide cancels out the INV_TIME_MULTIPLIER and converts the rate value to steps. + prep_segment->n_step = ceil(max(MINIMUM_STEP_RATE,st_prep_data->current_approx_rate)* + (ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND)/st_prep_data->dist_per_step); + // NOTE: Ensures it moves for very slow motions, but MINIMUM_STEP_RATE should always set this too. Perhaps + // a compile-time check to see if MINIMUM_STEP_RATE is set high enough is all that is needed. + prep_segment->n_step = max(prep_segment->n_step,MINIMUM_STEPS_PER_SEGMENT); + // NOTE: As long as the ACCELERATION_TICKS_PER_SECOND is valid, n_step should never exceed 255 and overflow. + // prep_segment->n_step = min(prep_segment->n_step,MAXIMUM_STEPS_PER_BLOCK); // Prevent unsigned int8 overflow. + + // Check if n_step exceeds steps remaining in planner block. If so, truncate. + if (prep_segment->n_step > st_prep_data->step_events_remaining) { + prep_segment->n_step = st_prep_data->step_events_remaining; + } + + // Check if n_step crosses decelerate point in block. If so, truncate to ensure the deceleration + // ramp counters are set correctly during execution. + if (st_prep_data->decelerate_after > 0) { + if (prep_segment->n_step > st_prep_data->decelerate_after) { + prep_segment->n_step = st_prep_data->decelerate_after; + } + } + + // Update stepper common data variables. + st_prep_data->decelerate_after -= prep_segment->n_step; + st_prep_data->step_events_remaining -= prep_segment->n_step; + + // Check for end of planner block + if ( st_prep_data->step_events_remaining == 0 ) { + + // TODO: When a feed hold ends, the step_events_remaining will also be zero, even though a block + // have partially been completed. We need to flag the stepper algorithm to indicate a stepper shutdown + // when complete, but not remove the planner block unless it truly is the end of the block (rare). + + // Set EOB bitflag so stepper algorithm discards the planner block after this segment completes. + prep_segment->flag |= SEGMENT_END_OF_BLOCK; + // Move planner pointer to next block and flag to load a new block for the next segment. + pl_prep_index = plan_next_block_index(pl_prep_index); + pl_prep_block = NULL; + } + + // New step segment completed. Increment segment buffer indices. + segment_buffer_head = segment_next_head; + if ( ++segment_next_head == SEGMENT_BUFFER_SIZE ) { segment_next_head = 0; } + +// long a = prep_segment->n_step; +// printInteger(a); +// printString(" "); + + } +} + +uint8_t st_get_prep_block_index() +{ +// Returns only the index but doesn't state if the block has been partially executed. How do we simply check for this? + return(pl_prep_index); +} + +void st_fetch_partial_block_parameters(uint8_t block_index, float *millimeters_remaining, uint8_t *is_decelerating) +{ + // if called, can we assume that this always changes and needs to be updated? if so, then + // we can perform all of the segment buffer setup tasks here to make sure the next time + // the segments are loaded, the st_data buffer is updated correctly. + // !!! Make sure that this is always pointing to the correct st_prep_data block. + + // When a mid-block acceleration occurs, we have to make sure the ramp counters are updated + // correctly, much in the same fashion as the deceleration counters. Need to think about this + // make sure this is right, but i'm pretty sure it is. + + // TODO: NULL means that the segment buffer has just completed a planner block. Clean up! + if (pl_prep_block != NULL) { + *millimeters_remaining = st_prep_data->step_events_remaining*st_prep_data->mm_per_step; + if (st_prep_data->decelerate_after > 0) { *is_decelerating = false; } + else { *is_decelerating = true; } + + // Flag for new prep_block when st_prep_buffer() is called after the planner recomputes. + pl_partial_block_flag = true; + pl_prep_block = NULL; } + return; } uint8_t st_is_decelerating() { diff --git a/stepper.h b/stepper.h index 2419a0b..8e2a6e0 100644 --- a/stepper.h +++ b/stepper.h @@ -45,7 +45,10 @@ void st_cycle_reinitialize(); // Initiates a feed hold of the running program void st_feed_hold(); -// Accessor function to query the acceleration state of the stepper -uint8_t st_is_decelerating(); +void st_prep_buffer(); + +uint8_t st_get_prep_block_index(); + +void st_fetch_partial_block_parameters(uint8_t block_index, float *millimeters_remaining, uint8_t *is_decelerating); #endif diff --git a/stepper_old.c b/stepper_old.c new file mode 100644 index 0000000..d702eba --- /dev/null +++ b/stepper_old.c @@ -0,0 +1,746 @@ +/* + stepper.c - stepper motor driver: executes motion plans using stepper motors + Part of Grbl + + Copyright (c) 2011-2013 Sungeun K. Jeon + Copyright (c) 2009-2011 Simen Svale Skogsrud + + 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 + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#include +#include "stepper.h" +#include "config.h" +#include "settings.h" +#include "planner.h" +#include "nuts_bolts.h" + +// Some useful constants +#define TICKS_PER_MICROSECOND (F_CPU/1000000) + +#define RAMP_NOOP_CRUISE 0 +#define RAMP_ACCEL 1 +#define RAMP_DECEL 2 + +#define LOAD_NOOP 0 +#define LOAD_SEGMENT 1 +#define LOAD_BLOCK 2 + +#define SEGMENT_NOOP 0 +#define SEGMENT_END_OF_BLOCK bit(0) +#define SEGMENT_ACCEL bit(1) +#define SEGMENT_DECEL bit(2) + +#define MINIMUM_STEPS_PER_SEGMENT 1 // Don't change + +#define SEGMENT_BUFFER_SIZE 6 + +// Stepper state variable. Contains running data and trapezoid variables. +typedef struct { + // Used by the bresenham line algorithm + int32_t counter_x, // Counter variables for the bresenham line tracer + counter_y, + counter_z; + uint8_t segment_steps_remaining; // Steps remaining in line segment motion + + // Used by inverse time algorithm to track step rate + int32_t counter_d; // Inverse time distance traveled since last step event + uint32_t delta_d; // Inverse time distance traveled per interrupt tick + uint32_t d_per_tick; + + // Used by the stepper driver interrupt + uint8_t execute_step; // Flags step execution for each interrupt. + uint8_t step_pulse_time; // Step pulse reset time after step rise + uint8_t out_bits; // The next stepping-bits to be output + uint8_t load_flag; + + uint8_t ramp_count; + uint8_t ramp_type; +} stepper_t; +static stepper_t st; + +// Stores stepper common data for executing steps in the segment buffer. Data can change mid-block when the +// planner updates the remaining block velocity profile with a more optimal plan or a feedrate override occurs. +// NOTE: Normally, this buffer is partially in-use, but, for the worst case scenario, it will never exceed +// the number of accessible stepper buffer segments (SEGMENT_BUFFER_SIZE-1). +typedef struct { + int32_t step_events_remaining; // Tracks step event count for the executing planner block + uint32_t d_next; // Scaled distance to next step + uint32_t initial_rate; // Initialized step rate at re/start of a planner block + uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute + uint32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive) + uint32_t current_approx_rate; // Tracks the approximate segment rate to predict steps per segment to execute + int32_t decelerate_after; // Tracks when to initiate deceleration according to the planner block + float mm_per_step; +} st_data_t; +static st_data_t segment_data[SEGMENT_BUFFER_SIZE-1]; + +// Primary stepper segment ring buffer. Contains small, short line segments for the stepper algorithm to execute, +// which are "checked-out" incrementally from the first block in the planner buffer. Once "checked-out", the steps +// in the segments buffer cannot be modified by the planner, where the remaining planner block steps still can. +typedef struct { + uint8_t n_step; // Number of step events to be executed for this segment + uint8_t st_data_index; // Stepper buffer common data index. Uses this information to execute this segment. + uint8_t flag; // Stepper algorithm bit-flag for special execution conditions. +} st_segment_t; +static st_segment_t segment_buffer[SEGMENT_BUFFER_SIZE]; + +// Step segment ring buffer indices +static volatile uint8_t segment_buffer_tail; +static volatile uint8_t segment_buffer_head; +static uint8_t segment_next_head; + +static volatile uint8_t busy; // Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though. +static plan_block_t *pl_current_block; // A pointer to the planner block currently being traced +static st_segment_t *st_current_segment; +static st_data_t *st_current_data; + +// Pointers for the step segment being prepped from the planner buffer. Accessed only by the +// main program. Pointers may be planning segments or planner blocks ahead of what being executed. +static plan_block_t *pl_prep_block; // Pointer to the planner block being prepped +static st_data_t *st_prep_data; // Pointer to the stepper common data being prepped +static uint8_t pl_prep_index; // Index of planner block being prepped +static uint8_t st_data_prep_index; // Index of stepper common data block being prepped +static uint8_t pl_partial_block_flag; // Flag indicating the planner has modified the prepped planner block + + +/* __________________________ + /| |\ _________________ ^ + / | | \ /| |\ | + / | | \ / | | \ s + / | | | | | \ p + / | | | | | \ e + +-----+------------------------+---+--+---------------+----+ e + | BLOCK 1 | BLOCK 2 | d + + time -----> + + The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta + until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after + after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as + +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. +*/ + +// Stepper state initialization. Cycle should only start if the st.cycle_start flag is +// enabled. Startup init and limits call this function but shouldn't start the cycle. +void st_wake_up() +{ + // Enable steppers by resetting the stepper disable port + if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { + STEPPERS_DISABLE_PORT |= (1<> 3); + // Enable stepper driver interrupt + st.execute_step = false; + st.load_flag = LOAD_BLOCK; + + TCNT2 = 0; // Clear Timer2 + TIMSK2 |= (1<n_step; + + // Check if the counters need to be reset for a new planner block + if (st.load_flag == LOAD_BLOCK) { + pl_current_block = plan_get_current_block(); // Should always be there. Stepper buffer handles this. + st_current_data = &segment_data[segment_buffer[segment_buffer_tail].st_data_index]; //st_current_segment->st_data_index]; + + // Initialize direction bits for block + st.out_bits = pl_current_block->direction_bits ^ settings.invert_mask; + st.execute_step = true; // Set flag to set direction bits upon next ISR tick. + + // Initialize Bresenham line counters + st.counter_x = (pl_current_block->step_event_count >> 1); + st.counter_y = st.counter_x; + st.counter_z = st.counter_x; + + // Initialize inverse time and step rate counter data + st.counter_d = st_current_data->d_next; // d_next always greater than delta_d. + if (st.delta_d < MINIMUM_STEP_RATE) { st.d_per_tick = MINIMUM_STEP_RATE; } + else { st.d_per_tick = st.delta_d; } + + // During feed hold, do not update rate, ramp type, or ramp counters. Keep decelerating. +// if (sys.state == STATE_CYCLE) { + st.delta_d = st_current_data->initial_rate; + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Initialize ramp counter via midpoint rule + st.ramp_type = RAMP_NOOP_CRUISE; // Initialize as no ramp operation. Corrected later if necessary. +// } + + } + + // Acceleration and cruise handled by ramping. Just check if deceleration needs to begin. + if ( st_current_segment->flag & (SEGMENT_DECEL | SEGMENT_ACCEL) ) { + /* Compute correct ramp count for a ramp change. Upon a switch from acceleration to deceleration, + or vice-versa, the new ramp count must be set to trigger the next acceleration tick equal to + the number of ramp ISR ticks counted since the last acceleration tick. This is ensures the + ramp is executed exactly as the plan dictates. Otherwise, when a ramp begins from a known + rate (nominal/cruise or initial), the ramp count must be set to ISR_TICKS_PER_ACCELERATION_TICK/2 + as mandated by the mid-point rule. For these conditions, the ramp count have been initialized + such that the following computation is still correct. */ + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; + if ( st_current_segment->flag & SEGMENT_DECEL ) { st.ramp_type = RAMP_DECEL; } + else { st.ramp_type = RAMP_ACCEL; } + } + + st.load_flag = LOAD_NOOP; // Segment motion loaded. Set no-operation flag to skip during execution. + + } else { + // Can't discard planner block here if a feed hold stops in middle of block. + st_go_idle(); + bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end + return; // Nothing to do but exit. + } + + } + + // Adjust inverse time counter for ac/de-celerations + // NOTE: Accelerations are handled by the stepper algorithm as it's thought to be more computationally + // efficient on the Arduino AVR. This could may not be true with higher ISR frequencies or faster CPUs. + if (st.ramp_type) { // Ignored when ramp type is NOOP_CRUISE + st.ramp_count--; // Tick acceleration ramp counter + if (st.ramp_count == 0) { // Adjust step rate when its time + if (st.ramp_type == RAMP_ACCEL) { // Adjust velocity for acceleration + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter + st.delta_d += st_current_data->rate_delta; + if (st.delta_d >= st_current_data->nominal_rate) { // Reached nominal rate. + st.delta_d = st_current_data->nominal_rate; // Set cruising velocity + st.ramp_type = RAMP_NOOP_CRUISE; // Set ramp flag to cruising + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Re-initialize counter for next ramp. + } + } else { // Adjust velocity for deceleration. + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter + if (st.delta_d > st_current_data->rate_delta) { + st.delta_d -= st_current_data->rate_delta; + } else { // Moving near zero feed rate. Gracefully slow down. + st.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow. + + // TODO: Check for and handle feed hold exit? At this point, machine is stopped. + // - Set system flag to recompute plan and reset segment buffer. + // - Segment steps in buffer needs to be returned to planner correctly. + // busy = false; + // return; + + } + } + // Finalize adjusted step rate. Ensure minimum. + if (st.delta_d < MINIMUM_STEP_RATE) { st.d_per_tick = MINIMUM_STEP_RATE; } + else { st.d_per_tick = st.delta_d; } + } + } + + // Iterate inverse time counter. Triggers each Bresenham step event. + st.counter_d -= st.d_per_tick; + + // Execute Bresenham step event, when it's time to do so. + if (st.counter_d < 0) { + st.counter_d += st_current_data->d_next; // Reload inverse time counter + + st.out_bits = pl_current_block->direction_bits; // Reset out_bits and reload direction bits + st.execute_step = true; + + // Execute step displacement profile by Bresenham line algorithm + st.counter_x -= pl_current_block->steps[X_AXIS]; + if (st.counter_x < 0) { + st.out_bits |= (1<step_event_count; + // st.steps_x++; + if (st.out_bits & (1<steps[Y_AXIS]; + if (st.counter_y < 0) { + st.out_bits |= (1<step_event_count; + // st.steps_y++; + if (st.out_bits & (1<steps[Z_AXIS]; + if (st.counter_z < 0) { + st.out_bits |= (1<step_event_count; + // st.steps_z++; + if (st.out_bits & (1< 0) { + if (st.out_bits & (1< 0) { + if (st.out_bits & (1< 0) { + if (st.out_bits & (1<flag & SEGMENT_END_OF_BLOCK) { + plan_discard_current_block(); + st.load_flag = LOAD_BLOCK; + } else { + st.load_flag = LOAD_SEGMENT; + } + + // Discard current segment by advancing buffer tail index + if ( ++segment_buffer_tail == SEGMENT_BUFFER_SIZE) { segment_buffer_tail = 0; } + + } + + st.out_bits ^= settings.invert_mask; // Apply step port invert mask + } + busy = false; +// SPINDLE_ENABLE_PORT ^= 1<step_events_remaining); +// st.ramp_type = RAMP_ACCEL; +// st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; +// st.delta_d = 0; +// sys.state = STATE_QUEUED; +// } else { +// sys.state = STATE_IDLE; +// } + sys.state = STATE_IDLE; + +} + + +/* Prepares step segment buffer. Continuously called from main program. + + The segment buffer is an intermediary buffer interface between the execution of steps + by the stepper algorithm and the velocity profiles generated by the planner. The stepper + algorithm only executes steps within the segment buffer and is filled by the main program + when steps are "checked-out" from the first block in the planner buffer. This keeps the + step execution and planning optimization processes atomic and protected from each other. + The number of steps "checked-out" from the planner buffer and the number of segments in + the segment buffer is sized and computed such that no operation in the main program takes + longer than the time it takes the stepper algorithm to empty it before refilling it. + Currently, the segment buffer conservatively holds roughly up to 40-60 msec of steps. + + NOTE: The segment buffer executes a set number of steps over an approximate time period. + If we try to execute over a set time period, it is difficult to guarantee or predict how + many steps will execute over it, especially when the step pulse phasing between the + neighboring segments are kept consistent. Meaning that, if the last segment step pulses + right before its end, the next segment must delay its first pulse so that the step pulses + are consistently spaced apart over time to keep the step pulse train nice and smooth. + Keeping track of phasing and ensuring that the exact number of steps are executed as + defined by the planner block, the related computational overhead gets quickly and + prohibitively expensive, especially in real-time. + Since the stepper algorithm automatically takes care of the step pulse phasing with + its ramp and inverse time counters, we don't have to explicitly and expensively track the + exact number of steps, time, or phasing of steps. All we need to do is approximate + the number of steps in each segment such that the segment buffer has enough execution time + for the main program to do what it needs to do and refill it when it has time. In other + words, we just need to compute a cheap approximation of the current velocity and the + number of steps over it. +*/ + +/* + TODO: Figure out how to enforce a deceleration when a feedrate override is reduced. + The problem is that when an override is reduced, the planner may not plan back to + the current rate. Meaning that the velocity profiles for certain conditions no longer + are trapezoidal or triangular. For example, if the current block is cruising at a + nominal rate and the feedrate override is reduced, the new nominal rate will now be + lower. The velocity profile must first decelerate to the new nominal rate and then + follow on the new plan. So the remaining velocity profile will have a decelerate, + cruise, and another decelerate. + Another issue is whether or not a feedrate override reduction causes a deceleration + that acts over several planner blocks. For example, say that the plan is already + heavily decelerating throughout it, reducing the feedrate will not do much to it. So, + how do we determine when to resume the new plan? How many blocks do we have to wait + until the new plan intersects with the deceleration curve? One plus though, the + deceleration will never be more than the number of blocks in the entire planner buffer, + but it theoretically can be equal to it when all planner blocks are decelerating already. +*/ +void st_prep_buffer() +{ + while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer. + + // Initialize new segment + st_segment_t *prep_segment = &segment_buffer[segment_buffer_head]; + prep_segment->flag = SEGMENT_NOOP; + + // Determine if we need to load a new planner block. + if (pl_prep_block == NULL) { + pl_prep_block = plan_get_block_by_index(pl_prep_index); // Query planner for a queued block + if (pl_prep_block == NULL) { return; } // No planner blocks. Exit. + + // Increment stepper common data index + if ( ++st_data_prep_index == (SEGMENT_BUFFER_SIZE-1) ) { st_data_prep_index = 0; } + + // Check if the planner has re-computed this block mid-execution. If so, push the previous segment + // data. Otherwise, prepare a new segment data for the new planner block. + if (pl_partial_block_flag) { + + // Prepare new shared segment block data and copy the relevant last segment block data. + st_data_t *last_st_prep_data; + last_st_prep_data = st_prep_data; + st_prep_data = &segment_data[st_data_prep_index]; + + st_prep_data->step_events_remaining = last_st_prep_data->step_events_remaining; + st_prep_data->rate_delta = last_st_prep_data->rate_delta; + st_prep_data->d_next = last_st_prep_data->d_next; + st_prep_data->nominal_rate = last_st_prep_data->nominal_rate; // TODO: Feedrate overrides recomputes this. + + st_prep_data->mm_per_step = last_st_prep_data->mm_per_step; + + pl_partial_block_flag = false; // Reset flag + + } else { + + // Prepare commonly shared planner block data for the ensuing segment buffer moves ad-hoc, since + // the planner buffer can dynamically change the velocity profile data as blocks are added. + st_prep_data = &segment_data[st_data_prep_index]; + + // Initialize Bresenham variables + st_prep_data->step_events_remaining = pl_prep_block->step_event_count; + + // Convert planner block velocity profile data to stepper rate and step distance data. + st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + st_prep_data->rate_delta = ceil(pl_prep_block->acceleration* + ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) + st_prep_data->d_next = ceil((pl_prep_block->millimeters*INV_TIME_MULTIPLIER)/pl_prep_block->step_event_count); // (mult*mm/step) + + // TODO: Check if we really need to store this. + st_prep_data->mm_per_step = pl_prep_block->millimeters/pl_prep_block->step_event_count; + + } + + // Convert planner entry speed to stepper initial rate. + st_prep_data->initial_rate = ceil(sqrt(pl_prep_block->entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + + // TODO: Nominal rate changes with feedrate override. + // st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + + st_prep_data->current_approx_rate = st_prep_data->initial_rate; + + // Calculate the planner block velocity profile type, determine deceleration point, and initial ramp. + float mm_decelerate_after = plan_calculate_velocity_profile(pl_prep_index); + st_prep_data->decelerate_after = ceil( mm_decelerate_after/st_prep_data->mm_per_step ); + if (st_prep_data->decelerate_after > 0) { // If 0, SEGMENT_DECEL flag is set later. + if (st_prep_data->initial_rate != st_prep_data->nominal_rate) { prep_segment->flag = SEGMENT_ACCEL; } + } + } + + // Set new segment to point to the current segment data block. + prep_segment->st_data_index = st_data_prep_index; + + // Approximate the velocity over the new segment using the already computed rate values. + // NOTE: This assumes that each segment will have an execution time roughly equal to every ACCELERATION_TICK. + // We do this to minimize memory and computational requirements. However, this could easily be replaced with + // a more exact approximation or have a unique time per segment, if CPU and memory overhead allows. + if (st_prep_data->decelerate_after <= 0) { + if (st_prep_data->decelerate_after == 0) { prep_segment->flag = SEGMENT_DECEL; } // Set segment deceleration flag + else { st_prep_data->current_approx_rate -= st_prep_data->rate_delta; } + if (st_prep_data->current_approx_rate < st_prep_data->rate_delta) { st_prep_data->current_approx_rate >>= 1; } + } else { + if (st_prep_data->current_approx_rate < st_prep_data->nominal_rate) { + st_prep_data->current_approx_rate += st_prep_data->rate_delta; + if (st_prep_data->current_approx_rate > st_prep_data->nominal_rate) { + st_prep_data->current_approx_rate = st_prep_data->nominal_rate; + } + } + } + + // Compute the number of steps in the prepped segment based on the approximate current rate. + // NOTE: The d_next divide cancels out the INV_TIME_MULTIPLIER and converts the rate value to steps. + prep_segment->n_step = ceil(max(MINIMUM_STEP_RATE,st_prep_data->current_approx_rate)* + (ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND)/st_prep_data->d_next); + // NOTE: Ensures it moves for very slow motions, but MINIMUM_STEP_RATE should always set this too. Perhaps + // a compile-time check to see if MINIMUM_STEP_RATE is set high enough is all that is needed. + prep_segment->n_step = max(prep_segment->n_step,MINIMUM_STEPS_PER_SEGMENT); + // NOTE: As long as the ACCELERATION_TICKS_PER_SECOND is valid, n_step should never exceed 255 and overflow. + // prep_segment->n_step = min(prep_segment->n_step,MAXIMUM_STEPS_PER_BLOCK); // Prevent unsigned int8 overflow. + + // Check if n_step exceeds steps remaining in planner block. If so, truncate. + if (prep_segment->n_step > st_prep_data->step_events_remaining) { + prep_segment->n_step = st_prep_data->step_events_remaining; + } + + // Check if n_step crosses decelerate point in block. If so, truncate to ensure the deceleration + // ramp counters are set correctly during execution. + if (st_prep_data->decelerate_after > 0) { + if (prep_segment->n_step > st_prep_data->decelerate_after) { + prep_segment->n_step = st_prep_data->decelerate_after; + } + } + + // Update stepper common data variables. + st_prep_data->decelerate_after -= prep_segment->n_step; + st_prep_data->step_events_remaining -= prep_segment->n_step; + + // Check for end of planner block + if ( st_prep_data->step_events_remaining == 0 ) { + // Set EOB bitflag so stepper algorithm discards the planner block after this segment completes. + prep_segment->flag |= SEGMENT_END_OF_BLOCK; + // Move planner pointer to next block and flag to load a new block for the next segment. + pl_prep_index = plan_next_block_index(pl_prep_index); + pl_prep_block = NULL; + } + + // New step segment completed. Increment segment buffer indices. + segment_buffer_head = segment_next_head; + if ( ++segment_next_head == SEGMENT_BUFFER_SIZE ) { segment_next_head = 0; } + +// long a = prep_segment->n_step; +// printInteger(a); +// printString(" "); + + } +} + +uint8_t st_get_prep_block_index() +{ +// Returns only the index but doesn't state if the block has been partially executed. How do we simply check for this? + return(pl_prep_index); +} + +void st_fetch_partial_block_parameters(uint8_t block_index, float *millimeters_remaining, uint8_t *is_decelerating) +{ + // if called, can we assume that this always changes and needs to be updated? if so, then + // we can perform all of the segment buffer setup tasks here to make sure the next time + // the segments are loaded, the st_data buffer is updated correctly. + // !!! Make sure that this is always pointing to the correct st_prep_data block. + + // When a mid-block acceleration occurs, we have to make sure the ramp counters are updated + // correctly, much in the same fashion as the deceleration counters. Need to think about this + // make sure this is right, but i'm pretty sure it is. + + // TODO: NULL means that the segment buffer has just completed a planner block. Clean up! + if (pl_prep_block != NULL) { + *millimeters_remaining = st_prep_data->step_events_remaining*st_prep_data->mm_per_step; + if (st_prep_data->decelerate_after > 0) { *is_decelerating = false; } + else { *is_decelerating = true; } + + // Flag for new prep_block when st_prep_buffer() is called after the planner recomputes. + pl_partial_block_flag = true; + pl_prep_block = NULL; + } + return; +} diff --git a/stepper_v0_9.c b/stepper_v0_9.c new file mode 100644 index 0000000..3191007 --- /dev/null +++ b/stepper_v0_9.c @@ -0,0 +1,387 @@ +/* + stepper.c - stepper motor driver: executes motion plans using stepper motors + Part of Grbl + + Copyright (c) 2011-2013 Sungeun K. Jeon + Copyright (c) 2009-2011 Simen Svale Skogsrud + + 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 + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +/* The timer calculations of this module informed by the 'RepRap cartesian firmware' by Zack Smith + and Philipp Tiefenbacher. */ + +#include +#include "stepper.h" +#include "config.h" +#include "settings.h" +#include "planner.h" + +// Some useful constants +#define TICKS_PER_MICROSECOND (F_CPU/1000000) +#define CRUISE_RAMP 0 +#define ACCEL_RAMP 1 +#define DECEL_RAMP 2 + +// Stepper state variable. Contains running data and trapezoid variables. +typedef struct { + // Used by the bresenham line algorithm + int32_t counter_x, // Counter variables for the bresenham line tracer + counter_y, + counter_z; + int32_t event_count; // Total event count. Retained for feed holds. + int32_t step_events_remaining; // Steps remaining in motion + + // Used by Pramod Ranade inverse time algorithm + int32_t delta_d; // Ranade distance traveled per interrupt tick + int32_t d_counter; // Ranade distance traveled since last step event + uint8_t ramp_count; // Acceleration interrupt tick counter. + uint8_t ramp_type; // Ramp type variable. + uint8_t execute_step; // Flags step execution for each interrupt. + +} stepper_t; +static stepper_t st; +static block_t *current_block; // A pointer to the block currently being traced + +// Used by the stepper driver interrupt +static uint8_t step_pulse_time; // Step pulse reset time after step rise +static uint8_t out_bits; // The next stepping-bits to be output + +// NOTE: If the main interrupt is guaranteed to be complete before the next interrupt, then +// this blocking variable is no longer needed. Only here for safety reasons. +static volatile uint8_t busy; // True when "Stepper Driver Interrupt" is being serviced. Used to avoid retriggering that handler. + +// __________________________ +// /| |\ _________________ ^ +// / | | \ /| |\ | +// / | | \ / | | \ s +// / | | | | | \ p +// / | | | | | \ e +// +-----+------------------------+---+--+---------------+----+ e +// | BLOCK 1 | BLOCK 2 | d +// +// time -----> +// +// The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta +// until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after +// after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as +// +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. + + +// Stepper state initialization. Cycle should only start if the st.cycle_start flag is +// enabled. Startup init and limits call this function but shouldn't start the cycle. +void st_wake_up() +{ + // Enable steppers by resetting the stepper disable port + if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { + STEPPERS_DISABLE_PORT |= (1<> 3); + // Enable stepper driver interrupt + st.execute_step = false; + TCNT2 = 0; // Clear Timer2 + TIMSK2 |= (1<direction_bits ^ settings.invert_mask; + st.execute_step = true; // Set flag to set direction bits. + + // Initialize Bresenham variables + st.counter_x = (current_block->step_event_count >> 1); + st.counter_y = st.counter_x; + st.counter_z = st.counter_x; + st.event_count = current_block->step_event_count; + st.step_events_remaining = st.event_count; + + // During feed hold, do not update Ranade counter, rate, or ramp type. Keep decelerating. + if (sys.state == STATE_CYCLE) { + // Initialize Ranade variables + st.d_counter = current_block->d_next; + st.delta_d = current_block->initial_rate; + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; + + // Initialize ramp type. + if (st.step_events_remaining == current_block->decelerate_after) { st.ramp_type = DECEL_RAMP; } + else if (st.delta_d == current_block->nominal_rate) { st.ramp_type = CRUISE_RAMP; } + else { st.ramp_type = ACCEL_RAMP; } + } + + } else { + st_go_idle(); + bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end + return; // Nothing to do but exit. + } + } + + // Adjust inverse time counter for ac/de-celerations + if (st.ramp_type) { + // Tick acceleration ramp counter + st.ramp_count--; + if (st.ramp_count == 0) { + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter + if (st.ramp_type == ACCEL_RAMP) { // Adjust velocity for acceleration + st.delta_d += current_block->rate_delta; + if (st.delta_d >= current_block->nominal_rate) { // Reached cruise state. + st.ramp_type = CRUISE_RAMP; + st.delta_d = current_block->nominal_rate; // Set cruise velocity + } + } else if (st.ramp_type == DECEL_RAMP) { // Adjust velocity for deceleration + if (st.delta_d > current_block->rate_delta) { + st.delta_d -= current_block->rate_delta; + } else { + st.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow. + } + } + } + } + + // Iterate Pramod Ranade inverse time counter. Triggers each Bresenham step event. + if (st.delta_d < MINIMUM_STEP_RATE) { st.d_counter -= MINIMUM_STEP_RATE; } + else { st.d_counter -= st.delta_d; } + + // Execute Bresenham step event, when it's time to do so. + if (st.d_counter < 0) { + st.d_counter += current_block->d_next; + + // Check for feed hold state and execute accordingly. + if (sys.state == STATE_HOLD) { + if (st.ramp_type != DECEL_RAMP) { + st.ramp_type = DECEL_RAMP; + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; + } + if (st.delta_d <= current_block->rate_delta) { + st_go_idle(); + bit_true(sys.execute,EXEC_CYCLE_STOP); + return; + } + } + + // TODO: Vary Bresenham resolution for smoother motions or enable faster step rates (>20kHz). + + out_bits = current_block->direction_bits; // Reset out_bits and reload direction bits + st.execute_step = true; + + // Execute step displacement profile by Bresenham line algorithm + st.counter_x -= current_block->steps[X_AXIS]; + if (st.counter_x < 0) { + out_bits |= (1<steps[Y_AXIS]; + if (st.counter_y < 0) { + out_bits |= (1<steps[Z_AXIS]; + if (st.counter_z < 0) { + out_bits |= (1<decelerate_after) { + st.ramp_type = DECEL_RAMP; + if (st.step_events_remaining == current_block->decelerate_after) { + if (st.delta_d == current_block->nominal_rate) { + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid + } else { + st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; // Set ramp counter for triangle + } + } + } + } + } else { + // If current block is finished, reset pointer + current_block = NULL; + plan_discard_current_block(); + } + + out_bits ^= settings.invert_mask; // Apply step port invert mask + } + busy = false; +// SPINDLE_ENABLE_PORT ^= 1< Date: Tue, 29 Oct 2013 19:28:44 -0600 Subject: [PATCH 20/73] Merge fixes. --- limits.c | 1 - motion_control.c | 1 - planner.c | 5 +---- report.c | 2 -- report.h | 1 - stepper.c | 6 ------ 6 files changed, 1 insertion(+), 15 deletions(-) diff --git a/limits.c b/limits.c index 3b15353..4fde4c9 100644 --- a/limits.c +++ b/limits.c @@ -67,7 +67,6 @@ ISR(LIMIT_INT_vect) if (sys.state != STATE_ALARM) { if (bit_isfalse(sys.execute,EXEC_ALARM)) { mc_reset(); // Initiate system kill. - report_alarm_message(ALARM_HARD_LIMIT); sys.execute |= EXEC_CRIT_EVENT; // Indicate hard limit critical event } } diff --git a/motion_control.c b/motion_control.c index d417031..e420c98 100644 --- a/motion_control.c +++ b/motion_control.c @@ -35,7 +35,6 @@ #include "planner.h" #include "limits.h" #include "protocol.h" -#include "report.h" // Execute linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second // unless invert_feed_rate is true. Then the feed_rate means that the motion should be completed in diff --git a/planner.c b/planner.c index 18f7bc1..0afa018 100644 --- a/planner.c +++ b/planner.c @@ -22,18 +22,14 @@ /* The ring buffer implementation gleaned from the wiring_serial library by David A. Mellis. */ -#include -#include #include #include -#include #include "planner.h" #include "nuts_bolts.h" #include "stepper.h" #include "settings.h" #include "config.h" #include "protocol.h" -#include "motion_control.h" #define SOME_LARGE_VALUE 1.0E+38 // Used by rapids and acceleration maximization calculations. Just needs // to be larger than any feasible (mm/min)^2 or mm/sec^2 value. @@ -452,6 +448,7 @@ void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) } } + // TODO: Need to check this method handling zero junction speeds when starting from rest. if (block_buffer_head == block_buffer_tail) { diff --git a/report.c b/report.c index 8688e4f..e71ac54 100644 --- a/report.c +++ b/report.c @@ -92,8 +92,6 @@ void report_alarm_message(int8_t alarm_code) printPgmString(PSTR("Hard/soft limit")); break; case ALARM_ABORT_CYCLE: printPgmString(PSTR("Abort during cycle")); break; - case ALARM_SOFT_LIMIT: - printPgmString(PSTR("Soft Limit")); break; } printPgmString(PSTR(". MPos?\r\n")); delay_ms(500); // Force delay to ensure message clears serial write buffer. diff --git a/report.h b/report.h index 26af23d..7dcfc47 100644 --- a/report.h +++ b/report.h @@ -41,7 +41,6 @@ // Define Grbl alarm codes. Less than zero to distinguish alarm error from status error. #define ALARM_LIMIT_ERROR -1 #define ALARM_ABORT_CYCLE -2 -#define ALARM_SOFT_LIMIT -3 // Define Grbl feedback message codes. #define MESSAGE_CRITICAL_EVENT 1 diff --git a/stepper.c b/stepper.c index a70a744..e3e4b25 100644 --- a/stepper.c +++ b/stepper.c @@ -177,7 +177,6 @@ void st_go_idle() STEPPERS_DISABLE_PORT |= (1< Date: Tue, 29 Oct 2013 19:43:40 -0600 Subject: [PATCH 21/73] Another merge fix. --- Makefile | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/Makefile b/Makefile index 213be43..8ff9359 100644 --- a/Makefile +++ b/Makefile @@ -42,7 +42,7 @@ FUSES = -U hfuse:w:0xd2:m -U lfuse:w:0xff:m # Tune the lines below only if you know what you are doing: AVRDUDE = avrdude $(PROGRAMMER) -p $(DEVICE) -B 10 -F -COMPILE = avr-gcc -Wall -Os -DF_CPU=$(CLOCK) -mmcu=$(DEVICE) -I. -ffunction-sections --std=c99 +COMPILE = avr-gcc -Wall -Os -DF_CPU=$(CLOCK) -mmcu=$(DEVICE) -I. -ffunction-sections # symbolic targets: all: grbl.hex @@ -97,4 +97,3 @@ cpp: # include generated header dependencies -include $(OBJECTS:.o=.d) - From b36e30de2eab858b5840b49b62753c18844608f8 Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Fri, 22 Nov 2013 17:35:58 -0700 Subject: [PATCH 22/73] Yet another major stepper algorithm and planner overhaul. - Overhauled the stepper algorithm and planner again. This time concentrating on the decoupling of the stepper ISR completely. It is now dumb, relying on the segment generator to provide the number of steps to execute and how fast it needs to go. This freed up lots of memory as well because it made a lot tracked variables obsolete. - The segment generator now computes the velocity profile of the executing planner block on the fly in floating point math, instead of allowing the stepper algorithm to govern accelerations in the previous code. What this accomplishes is the ability and framework to (somewhat) easily install a different physics model for generating a velocity profile, i.e. s-curves. - Made some more planner enhancements and increased efficiency a bit. - The changes also did not increase the compiled size of Grbl, but decreased it slightly as well. - Cleaned up a lot of the commenting. - Still much to do, but this push works and still is missing feedholds (coming next.) --- archive/planner_dist.c | 669 ++++++++++++++++++ archive/planner_time_archive.c | 459 +++++++++++++ planner_old.c => archive/planner_v0_9.c | 0 planner_old.h => archive/planner_v0_9.h | 0 archive/stepper_dist.c | 706 +++++++++++++++++++ stepper_old.c => archive/stepper_old.c | 0 archive/stepper_time.c | 775 +++++++++++++++++++++ archive/stepper_time_archive.c | 823 +++++++++++++++++++++++ stepper_v0_9.c => archive/stepper_v0_9.c | 0 config.h | 12 +- planner.c | 377 ++--------- planner.h | 17 +- stepper.c | 769 +++++++++++---------- stepper.h | 12 +- stepper_test.c | 788 ++++++++++++++++++++++ 15 files changed, 4697 insertions(+), 710 deletions(-) create mode 100644 archive/planner_dist.c create mode 100644 archive/planner_time_archive.c rename planner_old.c => archive/planner_v0_9.c (100%) rename planner_old.h => archive/planner_v0_9.h (100%) create mode 100644 archive/stepper_dist.c rename stepper_old.c => archive/stepper_old.c (100%) create mode 100644 archive/stepper_time.c create mode 100644 archive/stepper_time_archive.c rename stepper_v0_9.c => archive/stepper_v0_9.c (100%) create mode 100644 stepper_test.c diff --git a/archive/planner_dist.c b/archive/planner_dist.c new file mode 100644 index 0000000..0afa018 --- /dev/null +++ b/archive/planner_dist.c @@ -0,0 +1,669 @@ +/* + planner.c - buffers movement commands and manages the acceleration profile plan + Part of Grbl + + Copyright (c) 2011-2013 Sungeun K. Jeon + Copyright (c) 2009-2011 Simen Svale Skogsrud + Copyright (c) 2011 Jens Geisler + + 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 + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +/* The ring buffer implementation gleaned from the wiring_serial library by David A. Mellis. */ + +#include +#include +#include "planner.h" +#include "nuts_bolts.h" +#include "stepper.h" +#include "settings.h" +#include "config.h" +#include "protocol.h" + +#define SOME_LARGE_VALUE 1.0E+38 // Used by rapids and acceleration maximization calculations. Just needs + // to be larger than any feasible (mm/min)^2 or mm/sec^2 value. + +static plan_block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instructions +static volatile uint8_t block_buffer_tail; // Index of the block to process now +static uint8_t block_buffer_head; // Index of the next block to be pushed +static uint8_t next_buffer_head; // Index of the next buffer head +static uint8_t block_buffer_planned; // Index of the optimally planned block + +// Define planner variables +typedef struct { + int32_t position[N_AXIS]; // The planner position of the tool in absolute steps. Kept separate + // from g-code position for movements requiring multiple line motions, + // i.e. arcs, canned cycles, and backlash compensation. + float previous_unit_vec[N_AXIS]; // Unit vector of previous path line segment + float previous_nominal_speed_sqr; // Nominal speed of previous path line segment +} planner_t; +static planner_t pl; + + +// Returns the index of the next block in the ring buffer. Also called by stepper segment buffer. +// NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication. +uint8_t plan_next_block_index(uint8_t block_index) +{ + block_index++; + if (block_index == BLOCK_BUFFER_SIZE) { block_index = 0; } + return(block_index); +} + + +// Returns the index of the previous block in the ring buffer +static uint8_t plan_prev_block_index(uint8_t block_index) +{ + if (block_index == 0) { block_index = BLOCK_BUFFER_SIZE; } + block_index--; + return(block_index); +} + + +// Update the entry speed and millimeters remaining to execute for a partially completed block. Called only +// when the planner knows it will be changing the conditions of this block. +// TODO: Set up to be called from planner calculations. Need supporting code framework still, i.e. checking +// and executing this only when necessary, combine with the block_buffer_safe pointer. +// TODO: This is very similar to the planner reinitialize after a feed hold. Could make this do double duty. +void plan_update_partial_block(uint8_t block_index, float exit_speed_sqr) +{ +// TODO: Need to make a condition to check if we need make these calculations. We don't if nothing has +// been executed or placed into segment buffer. This happens with the first block upon startup or if +// the segment buffer is exactly in between two blocks. Just check if the step_events_remaining is equal +// the total step_event_count in the block. If so, we don't have to do anything. + + // !!! block index is the same as block_buffer_safe. + // See if we can reduce this down to just requesting the millimeters remaining.. + uint8_t is_decelerating; + float millimeters_remaining = 0.0; + st_fetch_partial_block_parameters(block_index, &millimeters_remaining, &is_decelerating); + + if (millimeters_remaining != 0.0) { + // Point to current block partially executed by stepper algorithm + plan_block_t *partial_block = plan_get_block_by_index(block_index); + + // Compute the midway speed of the partially completely block at the end of the segment buffer. + if (is_decelerating) { // Block is decelerating + partial_block->entry_speed_sqr = exit_speed_sqr - 2*partial_block->acceleration*millimeters_remaining; + } else { // Block is accelerating or cruising + partial_block->entry_speed_sqr += 2*partial_block->acceleration*(partial_block->millimeters-millimeters_remaining); + partial_block->entry_speed_sqr = min(partial_block->entry_speed_sqr, partial_block->nominal_speed_sqr); + } + + // Update only the relevant planner block information so the planner can plan correctly. + partial_block->millimeters = millimeters_remaining; + partial_block->max_entry_speed_sqr = partial_block->entry_speed_sqr; // Not sure if this needs to be updated. + } +} + + +/* PLANNER SPEED DEFINITION + +--------+ <- current->nominal_speed + / \ + current->entry_speed -> + \ + | + <- next->entry_speed (aka exit speed) + +-------------+ + time --> + + Recalculates the motion plan according to the following basic guidelines: + + 1. Go over every feasible block sequentially in reverse order and calculate the junction speeds + (i.e. current->entry_speed) such that: + a. No junction speed exceeds the pre-computed maximum junction speed limit or nominal speeds of + neighboring blocks. + b. A block entry speed cannot exceed one reverse-computed from its exit speed (next->entry_speed) + with a maximum allowable deceleration over the block travel distance. + c. The last (or newest appended) block is planned from a complete stop (an exit speed of zero). + 2. Go over every block in chronological (forward) order and dial down junction speed values if + a. The exit speed exceeds the one forward-computed from its entry speed with the maximum allowable + acceleration over the block travel distance. + + When these stages are complete, the planner will have maximized the velocity profiles throughout the all + of the planner blocks, where every block is operating at its maximum allowable acceleration limits. In + other words, for all of the blocks in the planner, the plan is optimal and no further speed improvements + are possible. If a new block is added to the buffer, the plan is recomputed according to the said + guidelines for a new optimal plan. + + To increase computational efficiency of these guidelines, a set of planner block pointers have been + created to indicate stop-compute points for when the planner guidelines cannot logically make any further + changes or improvements to the plan when in normal operation and new blocks are streamed and added to the + planner buffer. For example, if a subset of sequential blocks in the planner have been planned and are + bracketed by junction velocities at their maximums (or by the first planner block as well), no new block + added to the planner buffer will alter the velocity profiles within them. So we no longer have to compute + them. Or, if a set of sequential blocks from the first block in the planner (or a optimal stop-compute + point) are all accelerating, they are all optimal and can not be altered by a new block added to the + planner buffer, as this will only further increase the plan speed to chronological blocks until a maximum + junction velocity is reached. However, if the operational conditions of the plan changes from infrequently + used feed holds or feedrate overrides, the stop-compute pointers will be reset and the entire plan is + recomputed as stated in the general guidelines. + + Planner buffer index mapping: + - block_buffer_tail: Points to the beginning of the planner buffer. First to be executed or being executed. + - block_buffer_head: Points to the buffer block after the last block in the buffer. Used to indicate whether + the buffer is full or empty. As described for standard ring buffers, this block is always empty. + - next_buffer_head: Points to next planner buffer block after the buffer head block. When equal to the + buffer tail, this indicates the buffer is full. + - block_buffer_safe: Points to the first sequential planner block for which it is safe to recompute, which + is defined to be where the stepper's step segment buffer ends. This may or may not be the buffer tail, + since the step segment buffer queues steps which may have not finished executing and could span a few + blocks, if the block moves are very short. + - block_buffer_planned: Points to the first buffer block after the last optimally planned block for normal + streaming operating conditions. Use for planning optimizations by avoiding recomputing parts of the + planner buffer that don't change with the addition of a new block, as describe above. + + NOTE: All planner computations are performed in floating point to minimize numerical round-off errors. + When a planner block is executed, the floating point values are converted to fast integers by the stepper + algorithm segment buffer. See the stepper module for details. + + NOTE: Since the planner only computes on what's in the planner buffer, some motions with lots of short + line segments, like G2/3 arcs or complex curves, may seem to move slow. This is because there simply isn't + enough combined distance traveled in the entire buffer to accelerate up to the nominal speed and then + decelerate to a complete stop at the end of the buffer, as stated by the guidelines. If this happens and + becomes an annoyance, there are a few simple solutions: (1) Maximize the machine acceleration. The planner + will be able to compute higher velocity profiles within the same combined distance. (2) Maximize line + segment(s) distance per block to a desired tolerance. The more combined distance the planner has to use, + the faster it can go. (3) Maximize the planner buffer size. This also will increase the combined distance + for the planner to compute over. It also increases the number of computations the planner has to perform + to compute an optimal plan, so select carefully. The Arduino 328p memory is already maxed out, but future + ARM versions should have enough memory and speed for look-ahead blocks numbering up to a hundred or more. + +*/ +static void planner_recalculate() +{ + + // Initialize block index to the last block in the planner buffer. + uint8_t block_index = plan_prev_block_index(block_buffer_head); + + // Query stepper module for safe planner block index to recalculate to, which corresponds to the end + // of the step segment buffer. + uint8_t block_buffer_safe = st_get_prep_block_index(); + + // TODO: Make sure that we don't have to check for the block_buffer_tail condition, if the stepper module + // returns a NULL pointer or something. This could happen when the segment buffer is empty. Although, + // this call won't return a NULL, only an index.. I have to make sure that this index is synced with the + // planner at all times. + + // Recompute plan only when there is more than one planner block in the buffer. Can't do anything with one. + // NOTE: block_buffer_safe can be the last planner block if the segment buffer has completely queued up the + // remainder of the planner buffer. In this case, a new planner block will be treated as a single block. + if (block_index == block_buffer_safe) { // Also catches (head-1) = tail + + // Just set block_buffer_planned pointer. + block_buffer_planned = block_index; + + // TODO: Feedrate override of one block needs to update the partial block with an exit speed of zero. For + // a single added block and recalculate after a feed hold, we don't need to compute this, since we already + // know that the velocity starts and ends at zero. With an override, we can be traveling at some midblock + // rate, and we have to calculate the new velocity profile from it. + // plan_update_partial_block(block_index,0.0); + + } else { + + // TODO: If the nominal speeds change during a feedrate override, we need to recompute the max entry speeds for + // all junctions before proceeding. + + // Initialize planner buffer pointers and indexing. + plan_block_t *current = &block_buffer[block_index]; + + // Calculate maximum entry speed for last block in buffer, where the exit speed is always zero. + current->entry_speed_sqr = min( current->max_entry_speed_sqr, 2*current->acceleration*current->millimeters); + + // Reverse Pass: Coarsely maximize all possible deceleration curves back-planning from the last + // block in buffer. Cease planning when: (1) the last optimal planned pointer is reached. + // (2) the safe block pointer is reached, whereby the planned pointer is updated. + // NOTE: Forward pass will later refine and correct the reverse pass to create an optimal plan. + // NOTE: If the safe block is encountered before the planned block pointer, we know the safe block + // will be recomputed within the plan. So, we need to update it if it is partially completed. + float entry_speed_sqr; + plan_block_t *next; + block_index = plan_prev_block_index(block_index); + + if (block_index == block_buffer_safe) { // !! OR plan pointer? Yes I think so. + + // Only two plannable blocks in buffer. Compute previous block based on + // !!! May only work if a new block is being added. Not for an override. The exit speed isn't zero. + // !!! Need to make the current entry speed calculation after this. + plan_update_partial_block(block_index, 0.0); + block_buffer_planned = block_index; + + } else { + + // Three or more plan-able + while (block_index != block_buffer_planned) { + + next = current; + current = &block_buffer[block_index]; + + // Increment block index early to check if the safe block is before the current block. If encountered, + // this is an exit condition as we can't go further than this block in the reverse pass. + block_index = plan_prev_block_index(block_index); + if (block_index == block_buffer_safe) { + // Check if the safe block is partially completed. If so, update it before its exit speed + // (=current->entry speed) is over-written. + // TODO: The update breaks with feedrate overrides, because the replanning process no longer has + // the previous nominal speed to update this block with. There will need to be something along the + // lines of a nominal speed change check and send the correct value to this function. + plan_update_partial_block(block_index,current->entry_speed_sqr); + + // Set planned pointer at safe block and for loop exit after following computation is done. + block_buffer_planned = block_index; + } + + // Compute maximum entry speed decelerating over the current block from its exit speed. + if (current->entry_speed_sqr != current->max_entry_speed_sqr) { + entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters; + if (entry_speed_sqr < current->max_entry_speed_sqr) { + current->entry_speed_sqr = entry_speed_sqr; + } else { + current->entry_speed_sqr = current->max_entry_speed_sqr; + } + } + } + + } + + // Forward Pass: Forward plan the acceleration curve from the planned pointer onward. + // Also scans for optimal plan breakpoints and appropriately updates the planned pointer. + next = &block_buffer[block_buffer_planned]; // Begin at buffer planned pointer + block_index = plan_next_block_index(block_buffer_planned); + while (block_index != block_buffer_head) { + current = next; + next = &block_buffer[block_index]; + + // Any acceleration detected in the forward pass automatically moves the optimal planned + // pointer forward, since everything before this is all optimal. In other words, nothing + // can improve the plan from the buffer tail to the planned pointer by logic. + // TODO: Need to check if the planned flag logic is correct for all scenarios. It may not + // be for certain conditions. However, if the block reaches nominal speed, it can be a valid + // breakpoint substitute. + if (current->entry_speed_sqr < next->entry_speed_sqr) { + entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters; + // If true, current block is full-acceleration and we can move the planned pointer forward. + if (entry_speed_sqr < next->entry_speed_sqr) { + next->entry_speed_sqr = entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass sets this. + block_buffer_planned = block_index; // Set optimal plan pointer. + } + } + + // Any block set at its maximum entry speed also creates an optimal plan up to this + // point in the buffer. When the plan is bracketed by either the beginning of the + // buffer and a maximum entry speed or two maximum entry speeds, every block in between + // cannot logically be further improved. Hence, we don't have to recompute them anymore. + if (next->entry_speed_sqr == next->max_entry_speed_sqr) { + block_buffer_planned = block_index; // Set optimal plan pointer + } + + block_index = plan_next_block_index( block_index ); + } + + } + +} + + +void plan_reset_buffer() +{ + block_buffer_planned = block_buffer_tail; +} + + +void plan_init() +{ + block_buffer_tail = 0; + block_buffer_head = 0; // Empty = tail + next_buffer_head = 1; // plan_next_block_index(block_buffer_head) + plan_reset_buffer(); + memset(&pl, 0, sizeof(pl)); // Clear planner struct +} + + +void plan_discard_current_block() +{ + if (block_buffer_head != block_buffer_tail) { // Discard non-empty buffer. + block_buffer_tail = plan_next_block_index( block_buffer_tail ); + } +} + + +plan_block_t *plan_get_current_block() +{ + if (block_buffer_head == block_buffer_tail) { // Buffer empty + plan_reset_buffer(); + return(NULL); + } + return(&block_buffer[block_buffer_tail]); +} + + +plan_block_t *plan_get_block_by_index(uint8_t block_index) +{ + if (block_buffer_head == block_index) { return(NULL); } + return(&block_buffer[block_index]); +} + + +// Returns the availability status of the block ring buffer. True, if full. +uint8_t plan_check_full_buffer() +{ + if (block_buffer_tail == next_buffer_head) { return(true); } + return(false); +} + + +// Block until all buffered steps are executed or in a cycle state. Works with feed hold +// during a synchronize call, if it should happen. Also, waits for clean cycle end. +void plan_synchronize() +{ + while (plan_get_current_block() || sys.state == STATE_CYCLE) { + protocol_execute_runtime(); // Check and execute run-time commands + if (sys.abort) { return; } // Check for system abort + } +} + + +/* Add a new linear movement to the buffer. target[N_AXIS] is the signed, absolute target position + in millimeters. 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. + All position data passed to the planner must be in terms of machine position to keep the planner + independent of any coordinate system changes and offsets, which are handled by the g-code parser. + NOTE: Assumes buffer is available. Buffer checks are handled at a higher level by motion_control. + In other words, the buffer head is never equal to the buffer tail. Also the feed rate input value + is used in three ways: as a normal feed rate if invert_feed_rate is false, as inverse time if + invert_feed_rate is true, or as seek/rapids rate if the feed_rate value is negative (and + invert_feed_rate always false). */ +void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) +{ + // Prepare and initialize new block + plan_block_t *block = &block_buffer[block_buffer_head]; + block->step_event_count = 0; + block->millimeters = 0; + block->direction_bits = 0; + block->acceleration = SOME_LARGE_VALUE; // Scaled down to maximum acceleration later + + // Compute and store initial move distance data. + // TODO: After this for-loop, we don't touch the stepper algorithm data. Might be a good idea + // to try to keep these types of things completely separate from the planner for portability. + int32_t target_steps[N_AXIS]; + float unit_vec[N_AXIS], delta_mm; + uint8_t idx; + for (idx=0; idxsteps[idx] = labs(target_steps[idx]-pl.position[idx]); + block->step_event_count = max(block->step_event_count, block->steps[idx]); + + // Compute individual axes distance for move and prep unit vector calculations. + // NOTE: Computes true distance from converted step values. + delta_mm = (target_steps[idx] - pl.position[idx])/settings.steps_per_mm[idx]; + unit_vec[idx] = delta_mm; // Store unit vector numerator. Denominator computed later. + + // Set direction bits. Bit enabled always means direction is negative. + if (delta_mm < 0 ) { block->direction_bits |= get_direction_mask(idx); } + + // Incrementally compute total move distance by Euclidean norm. First add square of each term. + block->millimeters += delta_mm*delta_mm; + } + block->millimeters = sqrt(block->millimeters); // Complete millimeters calculation with sqrt() + + // Bail if this is a zero-length block. Highly unlikely to occur. + if (block->step_event_count == 0) { return; } + + // Adjust feed_rate value to mm/min depending on type of rate input (normal, inverse time, or rapids) + // TODO: Need to distinguish a rapids vs feed move for overrides. Some flag of some sort. + if (feed_rate < 0) { feed_rate = SOME_LARGE_VALUE; } // Scaled down to absolute max/rapids rate later + else if (invert_feed_rate) { feed_rate = block->millimeters/feed_rate; } + + // Calculate the unit vector of the line move and the block maximum feed rate and acceleration scaled + // down such that no individual axes maximum values are exceeded with respect to the line direction. + // NOTE: This calculation assumes all axes are orthogonal (Cartesian) and works with ABC-axes, + // if they are also orthogonal/independent. Operates on the absolute value of the unit vector. + float inverse_unit_vec_value; + float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple float divides + float junction_cos_theta = 0; + for (idx=0; idxacceleration = min(block->acceleration,settings.acceleration[idx]*inverse_unit_vec_value); + + // Incrementally compute cosine of angle between previous and current path. Cos(theta) of the junction + // between the current move and the previous move is simply the dot product of the two unit vectors, + // where prev_unit_vec is negative. Used later to compute maximum junction speed. + junction_cos_theta -= pl.previous_unit_vec[idx] * unit_vec[idx]; + } + } + + + // TODO: Need to check this method handling zero junction speeds when starting from rest. + if (block_buffer_head == block_buffer_tail) { + + // Initialize block entry speed as zero. Assume it will be starting from rest. Planner will correct this later. + block->entry_speed_sqr = 0.0; + block->max_junction_speed_sqr = 0.0; // Starting from rest. Enforce start from zero velocity. + + } else { + /* + Compute maximum allowable entry speed at junction by centripetal acceleration approximation. + 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 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. This approach does not actually deviate + from path, but used as a robust way to compute cornering speeds, as it takes into account the + nonlinearities of both the junction angle and junction velocity. + + NOTE: If the junction deviation value is finite, Grbl executes the motions in an exact path + mode (G61). If the junction deviation value is zero, Grbl will execute the motion in an exact + stop mode (G61.1) manner. In the future, if continuous mode (G64) is desired, the math here + is exactly the same. Instead of motioning all the way to junction point, the machine will + just follow the arc circle defined here. The Arduino doesn't have the CPU cycles to perform + a continuous mode path, but ARM-based microcontrollers most certainly do. + + NOTE: The max junction speed is a fixed value, since machine acceleration limits cannot be + changed dynamically during operation nor can the line move geometry. This must be kept in + memory in the event of a feedrate override changing the nominal speeds of blocks, which can + change the overall maximum entry speed conditions of all blocks. + */ + // NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta). + float sin_theta_d2 = sqrt(0.5*(1.0-junction_cos_theta)); // Trig half angle identity. Always positive. + + // TODO: Acceleration used in calculation needs to be limited by the minimum of the two junctions. + block->max_junction_speed_sqr = max( MINIMUM_JUNCTION_SPEED*MINIMUM_JUNCTION_SPEED, + (block->acceleration * settings.junction_deviation * sin_theta_d2)/(1.0-sin_theta_d2) ); + } + + // Store block nominal speed + block->nominal_speed_sqr = feed_rate*feed_rate; // (mm/min). Always > 0 + + // Compute the junction maximum entry based on the minimum of the junction speed and neighboring nominal speeds. + block->max_entry_speed_sqr = min(block->max_junction_speed_sqr, + min(block->nominal_speed_sqr,pl.previous_nominal_speed_sqr)); + + // Update previous path unit_vector and nominal speed (squared) + memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // pl.previous_unit_vec[] = unit_vec[] + pl.previous_nominal_speed_sqr = block->nominal_speed_sqr; + + // Update planner position + memcpy(pl.position, target_steps, sizeof(target_steps)); // pl.position[] = target_steps[] + + // New block is all set. Update buffer head and next buffer head indices. + block_buffer_head = next_buffer_head; + next_buffer_head = plan_next_block_index(block_buffer_head); + + // Finish up by recalculating the plan with the new block. + planner_recalculate(); + +// int32_t blength = block_buffer_head - block_buffer_tail; +// if (blength < 0) { blength += BLOCK_BUFFER_SIZE; } +// printInteger(blength); + + +} + + +// Reset the planner position vectors. Called by the system abort/initialization routine. +void plan_sync_position() +{ + uint8_t idx; + for (idx=0; idx + + +--------+ <- nominal_speed /|\ + / \ / | \ + entry_speed -> + \ / | + <- next->entry_speed + | + <- next->entry_speed / | | + +-------------+ entry_speed -> +----+--+ + time --> ^ ^ ^ ^ + | | | | + decelerate distance decelerate distance + + Calculates the type of velocity profile for a given planner block and provides the deceleration + distance for the stepper algorithm to use to accurately trace the profile exactly. The planner + computes the entry and exit speeds of each block, but does not bother to determine the details of + the velocity profiles within them, as they aren't needed for computing an optimal plan. When the + stepper algorithm begins to execute a block, the block velocity profiles are computed ad hoc. + + Each block velocity profiles can be described as either a trapezoidal or a triangular shape. The + trapezoid occurs when the block reaches the nominal speed of the block and cruises for a period of + time. A triangle occurs when the nominal speed is not reached within the block. Both of these + velocity profiles may also be truncated on either end with no acceleration or deceleration ramps, + as they can be influenced by the conditions of neighboring blocks, where the acceleration ramps + are defined by constant acceleration equal to the maximum allowable acceleration of a block. + + Since the stepper algorithm already assumes to begin executing a planner block by accelerating + from the planner entry speed and cruise if the nominal speed is reached, we only need to know + when to begin deceleration to the end of the block. Hence, only the distance from the end of the + block to begin a deceleration ramp is computed for the stepper algorithm when requested. +*/ +float plan_calculate_velocity_profile(uint8_t block_index) +{ + plan_block_t *current_block = &block_buffer[block_index]; + + // Determine current block exit speed + float exit_speed_sqr = 0.0; // Initialize for end of planner buffer. Zero speed. + plan_block_t *next_block = plan_get_block_by_index(plan_next_block_index(block_index)); + if (next_block != NULL) { exit_speed_sqr = next_block->entry_speed_sqr; } // Exit speed is the entry speed of next buffer block + + // First determine intersection distance (in steps) from the exit point for a triangular profile. + // Computes: d_intersect = distance/2 + (v_entry^2-v_exit^2)/(4*acceleration) + float intersect_distance = 0.5*( current_block->millimeters + (current_block->entry_speed_sqr-exit_speed_sqr)/(2*current_block->acceleration) ); + + // Check if this is a pure acceleration block by a intersection distance less than zero. Also + // prevents signed and unsigned integer conversion errors. + if (intersect_distance > 0 ) { + float decelerate_distance; + // Determine deceleration distance (in steps) from nominal speed to exit speed for a trapezoidal profile. + // Value is never negative. Nominal speed is always greater than or equal to the exit speed. + // Computes: d_decelerate = (v_nominal^2 - v_exit^2)/(2*acceleration) + decelerate_distance = (current_block->nominal_speed_sqr - exit_speed_sqr)/(2*current_block->acceleration); + + // The lesser of the two triangle and trapezoid distances always defines the velocity profile. + if (decelerate_distance > intersect_distance) { decelerate_distance = intersect_distance; } + + // Finally, check if this is a pure deceleration block. + if (decelerate_distance > current_block->millimeters) { return(0.0); } + else { return( (current_block->millimeters-decelerate_distance) ); } + } + return( current_block->millimeters ); // No deceleration in velocity profile. +} + + +// Re-initialize buffer plan with a partially completed block, assumed to exist at the buffer tail. +// Called after a steppers have come to a complete stop for a feed hold and the cycle is stopped. +void plan_cycle_reinitialize(int32_t step_events_remaining) +{ + plan_block_t *block = &block_buffer[block_buffer_tail]; // Point to partially completed block + + // Only remaining millimeters and step_event_count need to be updated for planner recalculate. + // Other variables (step_x, step_y, step_z, rate_delta, etc.) all need to remain the same to + // ensure the original planned motion is resumed exactly. + block->millimeters = (block->millimeters*step_events_remaining)/block->step_event_count; + block->step_event_count = step_events_remaining; + + // Re-plan from a complete stop. Reset planner entry speeds and buffer planned pointer. + block->entry_speed_sqr = 0.0; + block->max_entry_speed_sqr = 0.0; + block_buffer_planned = block_buffer_tail; + planner_recalculate(); +} + + +/* +TODO: + When a feed hold or feedrate override is reduced, the velocity profile must execute a + deceleration over the existing plan. By logic, since the plan already decelerates to zero + at the end of the buffer, any replanned deceleration mid-way will never exceed this. It + will only asymptotically approach this in the worst case scenario. + + - For a feed hold, we simply need to plan and compute the stopping point within a block + when velocity decelerates to zero. We then can recompute the plan with the already + existing partial block planning code and set the system to a QUEUED state. + - When a feed hold is initiated, the main program should be able to continue doing what + it has been, i.e. arcs, parsing, but needs to be able to reinitialize the plan after + it has come to a stop. + + - For a feed rate override (reduce-only), we need to enforce a deceleration until we + intersect the reduced nominal speed of a block after it's been planned with the new + overrides and the newly planned block is accelerating or cruising only. If the new plan + block is decelerating at the intersection point, we keep decelerating until we find a + valid intersection point. Once we find this point, we can then resume onto the new plan, + but we may need to adjust the deceleration point in the intersection block since the + feedrate override could have intersected at an acceleration ramp. This would change the + acceleration ramp to a cruising, so the deceleration point will have changed, but the + plan will have not. It should still be valid for the rest of the buffer. Coding this + can get complicated, but it should be doable. One issue could be is in how to handle + scenarios when a user issues several feedrate overrides and inundates this code. Does + this method still work and is robust enough to compute all of this on the fly? This is + the critical question. However, we could block user input until the planner has time to + catch to solve this as well. + + - When the feed rate override increases, we don't have to do anything special. We just + replan the entire buffer with the new nominal speeds and adjust the maximum junction + speeds accordingly. + +void plan_compute_deceleration() { + +} + + +void plan_recompute_max_junction_velocity() { + // Assumes the nominal_speed_sqr values have been updated. May need to just multiply + // override values here. + // PROBLEM: Axes-limiting velocities get screwed up. May need to store an int8 value for the + // max override value possible for each block when the line is added. So the nominal_speed + // is computed with that ceiling, but still retained if the rates change again. + uint8_t block_index = block_buffer_tail; + plan_block_t *block = &block_buffer[block_index]; + pl.previous_nominal_speed_sqr = block->nominal_speed_sqr; + block_index = plan_next_block_index(block_index); + while (block_index != block_buffer_head) { + block = &block_buffer[block_index]; + block->max_entry_speed_sqr = min(block->max_junction_speed_sqr, + min(block->nominal_speed_sqr,pl.previous_nominal_speed_sqr)); + pl.previous_nominal_speed_sqr = block->nominal_speed_sqr; + block_index = plan_next_block_index(block_index); + } +} + +*/ diff --git a/archive/planner_time_archive.c b/archive/planner_time_archive.c new file mode 100644 index 0000000..9f584df --- /dev/null +++ b/archive/planner_time_archive.c @@ -0,0 +1,459 @@ +/* + planner.c - buffers movement commands and manages the acceleration profile plan + Part of Grbl + + Copyright (c) 2011-2013 Sungeun K. Jeon + Copyright (c) 2009-2011 Simen Svale Skogsrud + Copyright (c) 2011 Jens Geisler + + 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 + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +/* The ring buffer implementation gleaned from the wiring_serial library by David A. Mellis. */ + +#include +#include +#include "planner.h" +#include "nuts_bolts.h" +#include "stepper.h" +#include "settings.h" +#include "config.h" +#include "protocol.h" + +#define SOME_LARGE_VALUE 1.0E+38 // Used by rapids and acceleration maximization calculations. Just needs + // to be larger than any feasible (mm/min)^2 or mm/sec^2 value. + +static plan_block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instructions +static uint8_t block_buffer_tail; // Index of the block to process now +static uint8_t block_buffer_head; // Index of the next block to be pushed +static uint8_t next_buffer_head; // Index of the next buffer head +static uint8_t block_buffer_planned; // Index of the optimally planned block + +// Define planner variables +typedef struct { + int32_t position[N_AXIS]; // The planner position of the tool in absolute steps. Kept separate + // from g-code position for movements requiring multiple line motions, + // i.e. arcs, canned cycles, and backlash compensation. + float previous_unit_vec[N_AXIS]; // Unit vector of previous path line segment + float previous_nominal_speed_sqr; // Nominal speed of previous path line segment +} planner_t; +static planner_t pl; + + +// Returns the index of the next block in the ring buffer. Also called by stepper segment buffer. +uint8_t plan_next_block_index(uint8_t block_index) +{ + block_index++; + if (block_index == BLOCK_BUFFER_SIZE) { block_index = 0; } + return(block_index); +} + + +// Returns the index of the previous block in the ring buffer +static uint8_t plan_prev_block_index(uint8_t block_index) +{ + if (block_index == 0) { block_index = BLOCK_BUFFER_SIZE; } + block_index--; + return(block_index); +} + + +/* PLANNER SPEED DEFINITION + +--------+ <- current->nominal_speed + / \ + current->entry_speed -> + \ + | + <- next->entry_speed (aka exit speed) + +-------------+ + time --> + + Recalculates the motion plan according to the following basic guidelines: + + 1. Go over every feasible block sequentially in reverse order and calculate the junction speeds + (i.e. current->entry_speed) such that: + a. No junction speed exceeds the pre-computed maximum junction speed limit or nominal speeds of + neighboring blocks. + b. A block entry speed cannot exceed one reverse-computed from its exit speed (next->entry_speed) + with a maximum allowable deceleration over the block travel distance. + c. The last (or newest appended) block is planned from a complete stop (an exit speed of zero). + 2. Go over every block in chronological (forward) order and dial down junction speed values if + a. The exit speed exceeds the one forward-computed from its entry speed with the maximum allowable + acceleration over the block travel distance. + + When these stages are complete, the planner will have maximized the velocity profiles throughout the all + of the planner blocks, where every block is operating at its maximum allowable acceleration limits. In + other words, for all of the blocks in the planner, the plan is optimal and no further speed improvements + are possible. If a new block is added to the buffer, the plan is recomputed according to the said + guidelines for a new optimal plan. + + To increase computational efficiency of these guidelines, a set of planner block pointers have been + created to indicate stop-compute points for when the planner guidelines cannot logically make any further + changes or improvements to the plan when in normal operation and new blocks are streamed and added to the + planner buffer. For example, if a subset of sequential blocks in the planner have been planned and are + bracketed by junction velocities at their maximums (or by the first planner block as well), no new block + added to the planner buffer will alter the velocity profiles within them. So we no longer have to compute + them. Or, if a set of sequential blocks from the first block in the planner (or a optimal stop-compute + point) are all accelerating, they are all optimal and can not be altered by a new block added to the + planner buffer, as this will only further increase the plan speed to chronological blocks until a maximum + junction velocity is reached. However, if the operational conditions of the plan changes from infrequently + used feed holds or feedrate overrides, the stop-compute pointers will be reset and the entire plan is + recomputed as stated in the general guidelines. + + Planner buffer index mapping: + - block_buffer_tail: Points to the beginning of the planner buffer. First to be executed or being executed. + - block_buffer_head: Points to the buffer block after the last block in the buffer. Used to indicate whether + the buffer is full or empty. As described for standard ring buffers, this block is always empty. + - next_buffer_head: Points to next planner buffer block after the buffer head block. When equal to the + buffer tail, this indicates the buffer is full. + - block_buffer_planned: Points to the first buffer block after the last optimally planned block for normal + streaming operating conditions. Use for planning optimizations by avoiding recomputing parts of the + planner buffer that don't change with the addition of a new block, as describe above. + + NOTE: Since the planner only computes on what's in the planner buffer, some motions with lots of short + line segments, like G2/3 arcs or complex curves, may seem to move slow. This is because there simply isn't + enough combined distance traveled in the entire buffer to accelerate up to the nominal speed and then + decelerate to a complete stop at the end of the buffer, as stated by the guidelines. If this happens and + becomes an annoyance, there are a few simple solutions: (1) Maximize the machine acceleration. The planner + will be able to compute higher velocity profiles within the same combined distance. (2) Maximize line + segment(s) distance per block to a desired tolerance. The more combined distance the planner has to use, + the faster it can go. (3) Maximize the planner buffer size. This also will increase the combined distance + for the planner to compute over. It also increases the number of computations the planner has to perform + to compute an optimal plan, so select carefully. The Arduino 328p memory is already maxed out, but future + ARM versions should have enough memory and speed for look-ahead blocks numbering up to a hundred or more. + +*/ +static void planner_recalculate() +{ + // Initialize block index to the last block in the planner buffer. + uint8_t block_index = plan_prev_block_index(block_buffer_head); + + // Recompute plan only when there is more than one planner block in the buffer. Can't do anything with one. + if (block_index == block_buffer_tail) { + // Just set block_buffer_planned pointer. + block_buffer_planned = block_buffer_tail; + return; + } + + // Initialize planner buffer pointers and indexing. + plan_block_t *current = &block_buffer[block_index]; + + // Calculate maximum entry speed for last block in buffer, where the exit speed is always zero. + current->entry_speed_sqr = min( current->max_entry_speed_sqr, 2*current->acceleration*current->millimeters); + + // Reverse Pass: Coarsely maximize all possible deceleration curves back-planning from the last + // block in buffer. Cease planning when: (1) the last optimal planned pointer is reached. + // (2) the safe block pointer is reached, whereby the planned pointer is updated. + // NOTE: Forward pass will later refine and correct the reverse pass to create an optimal plan. + // NOTE: If the safe block is encountered before the planned block pointer, we know the safe block + // will be recomputed within the plan. So, we need to update it if it is partially completed. + float entry_speed_sqr; + plan_block_t *next; + block_index = plan_prev_block_index(block_index); + if (block_index == block_buffer_tail) { // !! OR plan pointer? Yes I think so. + // Only two plannable blocks in buffer. Compute previous block based on + // !!! May only work if a new block is being added. Not for an override. The exit speed isn't zero. + // !!! Need to make the current entry speed calculation after this. + st_update_plan_block_parameters(); + block_buffer_planned = block_buffer_tail; + } else { + // Three or more plan-able blocks + while (block_index != block_buffer_planned) { + next = current; + current = &block_buffer[block_index]; + + // Increment block index early to check if the safe block is before the current block. If encountered, + // this is an exit condition as we can't go further than this block in the reverse pass. + block_index = plan_prev_block_index(block_index); + if (block_index == block_buffer_tail) { + // Check if the safe block is partially completed. If so, update it before its exit speed + // (=current->entry speed) is over-written. + // TODO: The update breaks with feedrate overrides, because the replanning process no longer has + // the previous nominal speed to update this block with. There will need to be something along the + // lines of a nominal speed change check and send the correct value to this function. + st_update_plan_block_parameters(); + + // Set planned pointer at safe block and for loop exit after following computation is done. + block_buffer_planned = block_buffer_tail; + } + + // Compute maximum entry speed decelerating over the current block from its exit speed. + if (current->entry_speed_sqr != current->max_entry_speed_sqr) { + entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters; + if (entry_speed_sqr < current->max_entry_speed_sqr) { + current->entry_speed_sqr = entry_speed_sqr; + } else { + current->entry_speed_sqr = current->max_entry_speed_sqr; + } + } + } + } + + // Forward Pass: Forward plan the acceleration curve from the planned pointer onward. + // Also scans for optimal plan breakpoints and appropriately updates the planned pointer. + next = &block_buffer[block_buffer_planned]; // Begin at buffer planned pointer + block_index = plan_next_block_index(block_buffer_planned); + while (block_index != block_buffer_head) { + current = next; + next = &block_buffer[block_index]; + + // Any acceleration detected in the forward pass automatically moves the optimal planned + // pointer forward, since everything before this is all optimal. In other words, nothing + // can improve the plan from the buffer tail to the planned pointer by logic. + // TODO: Need to check if the planned flag logic is correct for all scenarios. It may not + // be for certain conditions. However, if the block reaches nominal speed, it can be a valid + // breakpoint substitute. + if (current->entry_speed_sqr < next->entry_speed_sqr) { + entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters; + // If true, current block is full-acceleration and we can move the planned pointer forward. + if (entry_speed_sqr < next->entry_speed_sqr) { + next->entry_speed_sqr = entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass sets this. + block_buffer_planned = block_index; // Set optimal plan pointer. + } + } + + // Any block set at its maximum entry speed also creates an optimal plan up to this + // point in the buffer. When the plan is bracketed by either the beginning of the + // buffer and a maximum entry speed or two maximum entry speeds, every block in between + // cannot logically be further improved. Hence, we don't have to recompute them anymore. + if (next->entry_speed_sqr == next->max_entry_speed_sqr) { + block_buffer_planned = block_index; // Set optimal plan pointer + } + block_index = plan_next_block_index( block_index ); + } +} + + +void plan_init() +{ + memset(&pl, 0, sizeof(pl)); // Clear planner struct + block_buffer_tail = 0; + block_buffer_head = 0; // Empty = tail + next_buffer_head = 1; // plan_next_block_index(block_buffer_head) + block_buffer_planned = 0; // = block_buffer_tail; +} + + +void plan_discard_current_block() +{ + if (block_buffer_head != block_buffer_tail) { // Discard non-empty buffer. + block_buffer_tail = plan_next_block_index( block_buffer_tail ); + } +} + + +plan_block_t *plan_get_current_block() +{ + if (block_buffer_head == block_buffer_tail) { return(NULL); } // Buffer empty + return(&block_buffer[block_buffer_tail]); +} + + +float plan_get_exec_block_exit_speed() +{ + uint8_t block_index = plan_next_block_index(block_buffer_tail); + if (block_index == block_buffer_head) { return( 0.0 ); } + return( sqrt( block_buffer[block_index].entry_speed_sqr ) ); +} + + +// Returns the availability status of the block ring buffer. True, if full. +uint8_t plan_check_full_buffer() +{ + if (block_buffer_tail == next_buffer_head) { return(true); } + return(false); +} + + +// Block until all buffered steps are executed or in a cycle state. Works with feed hold +// during a synchronize call, if it should happen. Also, waits for clean cycle end. +void plan_synchronize() +{ + while (plan_get_current_block() || sys.state == STATE_CYCLE) { + protocol_execute_runtime(); // Check and execute run-time commands + if (sys.abort) { return; } // Check for system abort + } +} + + +/* Add a new linear movement to the buffer. target[N_AXIS] is the signed, absolute target position + in millimeters. 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. + All position data passed to the planner must be in terms of machine position to keep the planner + independent of any coordinate system changes and offsets, which are handled by the g-code parser. + NOTE: Assumes buffer is available. Buffer checks are handled at a higher level by motion_control. + In other words, the buffer head is never equal to the buffer tail. Also the feed rate input value + is used in three ways: as a normal feed rate if invert_feed_rate is false, as inverse time if + invert_feed_rate is true, or as seek/rapids rate if the feed_rate value is negative (and + invert_feed_rate always false). */ +void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) +{ + // Prepare and initialize new block + plan_block_t *block = &block_buffer[block_buffer_head]; + block->step_event_count = 0; + block->millimeters = 0; + block->direction_bits = 0; + block->acceleration = SOME_LARGE_VALUE; // Scaled down to maximum acceleration later + + // Compute and store initial move distance data. + // TODO: After this for-loop, we don't touch the stepper algorithm data. Might be a good idea + // to try to keep these types of things completely separate from the planner for portability. + int32_t target_steps[N_AXIS]; + float unit_vec[N_AXIS], delta_mm; + uint8_t idx; + for (idx=0; idxsteps[idx] = labs(target_steps[idx]-pl.position[idx]); + block->step_event_count = max(block->step_event_count, block->steps[idx]); + + // Compute individual axes distance for move and prep unit vector calculations. + // NOTE: Computes true distance from converted step values. + delta_mm = (target_steps[idx] - pl.position[idx])/settings.steps_per_mm[idx]; + unit_vec[idx] = delta_mm; // Store unit vector numerator. Denominator computed later. + + // Set direction bits. Bit enabled always means direction is negative. + if (delta_mm < 0 ) { block->direction_bits |= get_direction_mask(idx); } + + // Incrementally compute total move distance by Euclidean norm. First add square of each term. + block->millimeters += delta_mm*delta_mm; + } + block->millimeters = sqrt(block->millimeters); // Complete millimeters calculation with sqrt() + + // Bail if this is a zero-length block. Highly unlikely to occur. + if (block->step_event_count == 0) { return; } + + // Adjust feed_rate value to mm/min depending on type of rate input (normal, inverse time, or rapids) + // TODO: Need to distinguish a rapids vs feed move for overrides. Some flag of some sort. + if (feed_rate < 0) { feed_rate = SOME_LARGE_VALUE; } // Scaled down to absolute max/rapids rate later + else if (invert_feed_rate) { feed_rate = block->millimeters/feed_rate; } + + // Calculate the unit vector of the line move and the block maximum feed rate and acceleration scaled + // down such that no individual axes maximum values are exceeded with respect to the line direction. + // NOTE: This calculation assumes all axes are orthogonal (Cartesian) and works with ABC-axes, + // if they are also orthogonal/independent. Operates on the absolute value of the unit vector. + float inverse_unit_vec_value; + float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple float divides + float junction_cos_theta = 0; + for (idx=0; idxacceleration = min(block->acceleration,settings.acceleration[idx]*inverse_unit_vec_value); + + // Incrementally compute cosine of angle between previous and current path. Cos(theta) of the junction + // between the current move and the previous move is simply the dot product of the two unit vectors, + // where prev_unit_vec is negative. Used later to compute maximum junction speed. + junction_cos_theta -= pl.previous_unit_vec[idx] * unit_vec[idx]; + } + } + + + // TODO: Need to check this method handling zero junction speeds when starting from rest. + if (block_buffer_head == block_buffer_tail) { + + // Initialize block entry speed as zero. Assume it will be starting from rest. Planner will correct this later. + block->entry_speed_sqr = 0.0; + block->max_junction_speed_sqr = 0.0; // Starting from rest. Enforce start from zero velocity. + + } else { + /* + Compute maximum allowable entry speed at junction by centripetal acceleration approximation. + 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 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. This approach does not actually deviate + from path, but used as a robust way to compute cornering speeds, as it takes into account the + nonlinearities of both the junction angle and junction velocity. + + NOTE: If the junction deviation value is finite, Grbl executes the motions in an exact path + mode (G61). If the junction deviation value is zero, Grbl will execute the motion in an exact + stop mode (G61.1) manner. In the future, if continuous mode (G64) is desired, the math here + is exactly the same. Instead of motioning all the way to junction point, the machine will + just follow the arc circle defined here. The Arduino doesn't have the CPU cycles to perform + a continuous mode path, but ARM-based microcontrollers most certainly do. + + NOTE: The max junction speed is a fixed value, since machine acceleration limits cannot be + changed dynamically during operation nor can the line move geometry. This must be kept in + memory in the event of a feedrate override changing the nominal speeds of blocks, which can + change the overall maximum entry speed conditions of all blocks. + */ + // NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta). + float sin_theta_d2 = sqrt(0.5*(1.0-junction_cos_theta)); // Trig half angle identity. Always positive. + + // TODO: Acceleration used in calculation needs to be limited by the minimum of the two junctions. + block->max_junction_speed_sqr = max( MINIMUM_JUNCTION_SPEED*MINIMUM_JUNCTION_SPEED, + (block->acceleration * settings.junction_deviation * sin_theta_d2)/(1.0-sin_theta_d2) ); + } + + // Store block nominal speed + block->nominal_speed_sqr = feed_rate*feed_rate; // (mm/min). Always > 0 + + // Compute the junction maximum entry based on the minimum of the junction speed and neighboring nominal speeds. + block->max_entry_speed_sqr = min(block->max_junction_speed_sqr, + min(block->nominal_speed_sqr,pl.previous_nominal_speed_sqr)); + + // Update previous path unit_vector and nominal speed (squared) + memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // pl.previous_unit_vec[] = unit_vec[] + pl.previous_nominal_speed_sqr = block->nominal_speed_sqr; + + // Update planner position + memcpy(pl.position, target_steps, sizeof(target_steps)); // pl.position[] = target_steps[] + + // New block is all set. Update buffer head and next buffer head indices. + block_buffer_head = next_buffer_head; + next_buffer_head = plan_next_block_index(block_buffer_head); + + // Finish up by recalculating the plan with the new block. + planner_recalculate(); + +// int32_t blength = block_buffer_head - block_buffer_tail; +// if (blength < 0) { blength += BLOCK_BUFFER_SIZE; } +// printInteger(blength); +} + + +// Reset the planner position vectors. Called by the system abort/initialization routine. +void plan_sync_position() +{ + uint8_t idx; + for (idx=0; idxmillimeters = (block->millimeters*step_events_remaining)/block->step_event_count; + block->step_event_count = step_events_remaining; + + // Re-plan from a complete stop. Reset planner entry speeds and buffer planned pointer. + block->entry_speed_sqr = 0.0; + block->max_entry_speed_sqr = 0.0; + block_buffer_planned = block_buffer_tail; + planner_recalculate(); +} diff --git a/planner_old.c b/archive/planner_v0_9.c similarity index 100% rename from planner_old.c rename to archive/planner_v0_9.c diff --git a/planner_old.h b/archive/planner_v0_9.h similarity index 100% rename from planner_old.h rename to archive/planner_v0_9.h diff --git a/archive/stepper_dist.c b/archive/stepper_dist.c new file mode 100644 index 0000000..fe74e73 --- /dev/null +++ b/archive/stepper_dist.c @@ -0,0 +1,706 @@ +/* + stepper.c - stepper motor driver: executes motion plans using stepper motors + Part of Grbl + + Copyright (c) 2011-2013 Sungeun K. Jeon + Copyright (c) 2009-2011 Simen Svale Skogsrud + + 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 + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#include +#include "stepper.h" +#include "config.h" +#include "settings.h" +#include "planner.h" +#include "nuts_bolts.h" + +// Some useful constants +#define TICKS_PER_MICROSECOND (F_CPU/1000000) + +#define RAMP_NOOP_CRUISE 0 +#define RAMP_ACCEL 1 +#define RAMP_DECEL 2 + +#define LOAD_NOOP 0 +#define LOAD_SEGMENT 1 +#define LOAD_BLOCK 2 + +#define SEGMENT_NOOP 0 +#define SEGMENT_END_OF_BLOCK bit(0) +#define RAMP_CHANGE_ACCEL bit(1) +#define RAMP_CHANGE_DECEL bit(2) + +#define MINIMUM_STEPS_PER_SEGMENT 1 // Don't change + +#define SEGMENT_BUFFER_SIZE 6 + +// Stepper state variable. Contains running data and trapezoid variables. +typedef struct { + // Used by the bresenham line algorithm + int32_t counter_x, // Counter variables for the bresenham line tracer + counter_y, + counter_z; + uint8_t segment_steps_remaining; // Steps remaining in line segment motion + + // Used by inverse time algorithm to track step rate + int32_t counter_dist; // Inverse time distance traveled since last step event + uint32_t ramp_rate; // Inverse time distance traveled per interrupt tick + uint32_t dist_per_tick; + + // Used by the stepper driver interrupt + uint8_t execute_step; // Flags step execution for each interrupt. + uint8_t step_pulse_time; // Step pulse reset time after step rise + uint8_t out_bits; // The next stepping-bits to be output + uint8_t load_flag; + + uint8_t counter_ramp; + uint8_t ramp_type; +} stepper_t; +static stepper_t st; + +// Stores stepper common data for executing steps in the segment buffer. Data can change mid-block when the +// planner updates the remaining block velocity profile with a more optimal plan or a feedrate override occurs. +// NOTE: Normally, this buffer is partially in-use, but, for the worst case scenario, it will never exceed +// the number of accessible stepper buffer segments (SEGMENT_BUFFER_SIZE-1). +typedef struct { + int32_t step_events_remaining; // Tracks step event count for the executing planner block + uint32_t dist_per_step; // Scaled distance to next step + uint32_t initial_rate; // Initialized step rate at re/start of a planner block + uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute + uint32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive) + uint32_t current_approx_rate; // Tracks the approximate segment rate to predict steps per segment to execute + int32_t decelerate_after; // Tracks when to initiate deceleration according to the planner block + float mm_per_step; +} st_data_t; +static st_data_t segment_data[SEGMENT_BUFFER_SIZE-1]; + +// Primary stepper segment ring buffer. Contains small, short line segments for the stepper algorithm to execute, +// which are "checked-out" incrementally from the first block in the planner buffer. Once "checked-out", the steps +// in the segments buffer cannot be modified by the planner, where the remaining planner block steps still can. +typedef struct { + uint8_t n_step; // Number of step events to be executed for this segment + uint8_t st_data_index; // Stepper buffer common data index. Uses this information to execute this segment. + uint8_t flag; // Stepper algorithm bit-flag for special execution conditions. +} st_segment_t; +static st_segment_t segment_buffer[SEGMENT_BUFFER_SIZE]; + +// Step segment ring buffer indices +static volatile uint8_t segment_buffer_tail; +static volatile uint8_t segment_buffer_head; +static uint8_t segment_next_head; + +static volatile uint8_t busy; // Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though. +static plan_block_t *pl_current_block; // A pointer to the planner block currently being traced +static st_segment_t *st_current_segment; +static st_data_t *st_current_data; + +// Pointers for the step segment being prepped from the planner buffer. Accessed only by the +// main program. Pointers may be planning segments or planner blocks ahead of what being executed. +static plan_block_t *pl_prep_block; // Pointer to the planner block being prepped +static st_data_t *st_prep_data; // Pointer to the stepper common data being prepped +static uint8_t pl_prep_index; // Index of planner block being prepped +static uint8_t st_data_prep_index; // Index of stepper common data block being prepped +static uint8_t pl_partial_block_flag; // Flag indicating the planner has modified the prepped planner block + + +/* __________________________ + /| |\ _________________ ^ + / | | \ /| |\ | + / | | \ / | | \ s + / | | | | | \ p + / | | | | | \ e + +-----+------------------------+---+--+---------------+----+ e + | BLOCK 1 | BLOCK 2 | d + + time -----> + + The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta + until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after + after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as + +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. +*/ + +// Stepper state initialization. Cycle should only start if the st.cycle_start flag is +// enabled. Startup init and limits call this function but shouldn't start the cycle. +void st_wake_up() +{ + // Enable steppers by resetting the stepper disable port + if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { + STEPPERS_DISABLE_PORT |= (1<> 3); + // Enable stepper driver interrupt + st.execute_step = false; + st.load_flag = LOAD_BLOCK; + + TCNT2 = 0; // Clear Timer2 + TIMSK2 |= (1<n_step; + + // If the new segment starts a new planner block, initialize stepper variables and counters. + // NOTE: For new segments only, the step counters are not updated to ensure step phasing is continuous. + if (st.load_flag == LOAD_BLOCK) { + pl_current_block = plan_get_current_block(); // Should always be there. Stepper buffer handles this. + st_current_data = &segment_data[segment_buffer[segment_buffer_tail].st_data_index]; + + // Initialize direction bits for block. Set execute flag to set directions bits upon next ISR tick. + st.out_bits = pl_current_block->direction_bits ^ settings.invert_mask; + st.execute_step = true; + + // Initialize Bresenham line counters + st.counter_x = (pl_current_block->step_event_count >> 1); + st.counter_y = st.counter_x; + st.counter_z = st.counter_x; + + // Initialize inverse time, step rate data, and acceleration ramp counters + st.counter_dist = st_current_data->dist_per_step; // dist_per_step always greater than ramp_rate. + st.ramp_rate = st_current_data->initial_rate; + st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK/2; // Initialize ramp counter via midpoint rule + st.ramp_type = RAMP_NOOP_CRUISE; // Initialize as no ramp operation. Corrected later if necessary. + + // Ensure the initial step rate exceeds the MINIMUM_STEP_RATE. + if (st.ramp_rate < MINIMUM_STEP_RATE) { st.dist_per_tick = MINIMUM_STEP_RATE; } + else { st.dist_per_tick = st.ramp_rate; } + } + + // Check if ramp conditions have changed. If so, update ramp counters and control variables. + if ( st_current_segment->flag & (RAMP_CHANGE_DECEL | RAMP_CHANGE_ACCEL) ) { + /* Compute correct ramp count for a ramp change. Upon a switch from acceleration to deceleration, + or vice-versa, the new ramp count must be set to trigger the next acceleration tick equal to + the number of ramp ISR ticks counted since the last acceleration tick. This is ensures the + ramp is executed exactly as the plan dictates. Otherwise, when a ramp begins from a known + rate (nominal/cruise or initial), the ramp count must be set to ISR_TICKS_PER_ACCELERATION_TICK/2 + as mandated by the mid-point rule. For the latter conditions, the ramp count have been + initialized such that the following computation is still correct. */ + st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK-st.counter_ramp; + if ( st_current_segment->flag & RAMP_CHANGE_DECEL ) { st.ramp_type = RAMP_DECEL; } + else { st.ramp_type = RAMP_ACCEL; } + } + + st.load_flag = LOAD_NOOP; // Segment motion loaded. Set no-operation flag to skip during execution. + + } else { + // Can't discard planner block here if a feed hold stops in middle of block. + st_go_idle(); + bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end + return; // Nothing to do but exit. + } + + } + + // Adjust inverse time counter for ac/de-celerations + if (st.ramp_type) { // Ignored when ramp type is RAMP_NOOP_CRUISE + st.counter_ramp--; // Tick acceleration ramp counter + if (st.counter_ramp == 0) { // Adjust step rate when its time + st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter + if (st.ramp_type == RAMP_ACCEL) { // Adjust velocity for acceleration + st.ramp_rate += st_current_data->rate_delta; + if (st.ramp_rate >= st_current_data->nominal_rate) { // Reached nominal rate. + st.ramp_rate = st_current_data->nominal_rate; // Set cruising velocity + st.ramp_type = RAMP_NOOP_CRUISE; // Set ramp flag to cruising + st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK/2; // Re-initialize counter for next ramp change. + } + } else { // Adjust velocity for deceleration. + if (st.ramp_rate > st_current_data->rate_delta) { + st.ramp_rate -= st_current_data->rate_delta; + } else { // Moving near zero feed rate. Gracefully slow down. + st.ramp_rate >>= 1; // Integer divide by 2 until complete. Also prevents overflow. + } + } + // Adjust for minimum step rate, but retain operating ramp rate for accurate velocity tracing. + if (st.ramp_rate < MINIMUM_STEP_RATE) { st.dist_per_tick = MINIMUM_STEP_RATE; } + else { st.dist_per_tick = st.ramp_rate; } + } + } + + // Iterate inverse time counter. Triggers each Bresenham step event. + st.counter_dist -= st.dist_per_tick; + + // Execute Bresenham step event, when it's time to do so. + if (st.counter_dist < 0) { + st.counter_dist += st_current_data->dist_per_step; // Reload inverse time counter + + st.out_bits = pl_current_block->direction_bits; // Reset out_bits and reload direction bits + st.execute_step = true; + + // Execute step displacement profile by Bresenham line algorithm + st.counter_x -= pl_current_block->steps[X_AXIS]; + if (st.counter_x < 0) { + st.out_bits |= (1<step_event_count; + if (st.out_bits & (1<steps[Y_AXIS]; + if (st.counter_y < 0) { + st.out_bits |= (1<step_event_count; + if (st.out_bits & (1<steps[Z_AXIS]; + if (st.counter_z < 0) { + st.out_bits |= (1<step_event_count; + if (st.out_bits & (1<flag & SEGMENT_END_OF_BLOCK) { + plan_discard_current_block(); + st.load_flag = LOAD_BLOCK; + } else { + st.load_flag = LOAD_SEGMENT; + } + + // Discard current segment by advancing buffer tail index + if ( ++segment_buffer_tail == SEGMENT_BUFFER_SIZE) { segment_buffer_tail = 0; } + } + + st.out_bits ^= settings.invert_mask; // Apply step port invert mask + } + busy = false; +// SPINDLE_ENABLE_PORT ^= 1<step_events_remaining); +// st.ramp_type = RAMP_ACCEL; +// st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK/2; +// st.ramp_rate = 0; +// sys.state = STATE_QUEUED; +// } else { +// sys.state = STATE_IDLE; +// } + sys.state = STATE_IDLE; + +} + + +/* Prepares step segment buffer. Continuously called from main program. + + The segment buffer is an intermediary buffer interface between the execution of steps + by the stepper algorithm and the velocity profiles generated by the planner. The stepper + algorithm only executes steps within the segment buffer and is filled by the main program + when steps are "checked-out" from the first block in the planner buffer. This keeps the + step execution and planning optimization processes atomic and protected from each other. + The number of steps "checked-out" from the planner buffer and the number of segments in + the segment buffer is sized and computed such that no operation in the main program takes + longer than the time it takes the stepper algorithm to empty it before refilling it. + Currently, the segment buffer conservatively holds roughly up to 40-60 msec of steps. + + NOTE: The segment buffer executes a set number of steps over an approximate time period. + If we try to execute over a fixed time period, it is difficult to guarantee or predict + how many steps will execute over it, especially when the step pulse phasing between the + neighboring segments must also be kept consistent. Meaning that, if the last segment step + pulses right before a segment end, the next segment must delay its first pulse so that the + step pulses are consistently spaced apart over time to keep the step pulse train nice and + smooth. Keeping track of phasing and ensuring that the exact number of steps are executed + as defined by the planner block, the related computational overhead can get quickly and + prohibitively expensive, especially in real-time. + Since the stepper algorithm automatically takes care of the step pulse phasing with + its ramp and inverse time counters by retaining the count remainders, we don't have to + explicitly and expensively track and synchronize the exact number of steps, time, and + phasing of steps. All we need to do is approximate the number of steps in each segment + such that the segment buffer has enough execution time for the main program to do what + it needs to do and refill it when it comes back. In other words, we just need to compute + a cheap approximation of the current velocity and the number of steps over it. +*/ + +/* + TODO: Figure out how to enforce a deceleration when a feedrate override is reduced. + The problem is that when an override is reduced, the planner may not plan back to + the current rate. Meaning that the velocity profiles for certain conditions no longer + are trapezoidal or triangular. For example, if the current block is cruising at a + nominal rate and the feedrate override is reduced, the new nominal rate will now be + lower. The velocity profile must first decelerate to the new nominal rate and then + follow on the new plan. So the remaining velocity profile will have a decelerate, + cruise, and another decelerate. + Another issue is whether or not a feedrate override reduction causes a deceleration + that acts over several planner blocks. For example, say that the plan is already + heavily decelerating throughout it, reducing the feedrate will not do much to it. So, + how do we determine when to resume the new plan? How many blocks do we have to wait + until the new plan intersects with the deceleration curve? One plus though, the + deceleration will never be more than the number of blocks in the entire planner buffer, + but it theoretically can be equal to it when all planner blocks are decelerating already. +*/ +void st_prep_buffer() +{ + if (sys.state == STATE_QUEUED) { return; } // Block until a motion state is issued + while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer. + + // Initialize new segment + st_segment_t *prep_segment = &segment_buffer[segment_buffer_head]; + prep_segment->flag = SEGMENT_NOOP; + + // Determine if we need to load a new planner block. + if (pl_prep_block == NULL) { + pl_prep_block = plan_get_block_by_index(pl_prep_index); // Query planner for a queued block + if (pl_prep_block == NULL) { return; } // No planner blocks. Exit. + + +SPINDLE_ENABLE_PORT ^= 1<step_events_remaining = last_st_prep_data->step_events_remaining; + st_prep_data->rate_delta = last_st_prep_data->rate_delta; + st_prep_data->dist_per_step = last_st_prep_data->dist_per_step; + st_prep_data->nominal_rate = last_st_prep_data->nominal_rate; // TODO: Feedrate overrides recomputes this. + + st_prep_data->mm_per_step = last_st_prep_data->mm_per_step; + + pl_partial_block_flag = false; // Reset flag + + } else { + + // Prepare commonly shared planner block data for the ensuing segment buffer moves ad-hoc, since + // the planner buffer can dynamically change the velocity profile data as blocks are added. + st_prep_data = &segment_data[st_data_prep_index]; + + // Initialize Bresenham variables + st_prep_data->step_events_remaining = pl_prep_block->step_event_count; + + // Convert planner block velocity profile data to stepper rate and step distance data. + st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + st_prep_data->rate_delta = ceil(pl_prep_block->acceleration* + ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) + st_prep_data->dist_per_step = ceil((pl_prep_block->millimeters*INV_TIME_MULTIPLIER)/pl_prep_block->step_event_count); // (mult*mm/step) + + // TODO: Check if we really need to store this. + st_prep_data->mm_per_step = pl_prep_block->millimeters/pl_prep_block->step_event_count; + + } + + // Convert planner entry speed to stepper initial rate. + st_prep_data->initial_rate = ceil(sqrt(pl_prep_block->entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + + // TODO: Nominal rate changes with feedrate override. + // st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + + st_prep_data->current_approx_rate = st_prep_data->initial_rate; + + // Calculate the planner block velocity profile type, determine deceleration point, and initial ramp. + float mm_decelerate_after = plan_calculate_velocity_profile(pl_prep_index); + st_prep_data->decelerate_after = ceil( mm_decelerate_after/st_prep_data->mm_per_step ); + if (st_prep_data->decelerate_after > 0) { // If 0, RAMP_CHANGE_DECEL flag is set later. + if (st_prep_data->initial_rate != st_prep_data->nominal_rate) { prep_segment->flag = RAMP_CHANGE_ACCEL; } + } + } + + // Set new segment to point to the current segment data block. + prep_segment->st_data_index = st_data_prep_index; + + // Approximate the velocity over the new segment using the already computed rate values. + // NOTE: This assumes that each segment will have an execution time roughly equal to every ACCELERATION_TICK. + // We do this to minimize memory and computational requirements. However, this could easily be replaced with + // a more exact approximation or have an user-defined time per segment, if CPU and memory overhead allows. + if (st_prep_data->decelerate_after <= 0) { + if (st_prep_data->decelerate_after == 0) { prep_segment->flag = RAMP_CHANGE_DECEL; } // Set segment deceleration flag + else { st_prep_data->current_approx_rate -= st_prep_data->rate_delta; } + if (st_prep_data->current_approx_rate < st_prep_data->rate_delta) { st_prep_data->current_approx_rate >>= 1; } + } else { + if (st_prep_data->current_approx_rate < st_prep_data->nominal_rate) { + st_prep_data->current_approx_rate += st_prep_data->rate_delta; + if (st_prep_data->current_approx_rate > st_prep_data->nominal_rate) { + st_prep_data->current_approx_rate = st_prep_data->nominal_rate; + } + } + } + + // TODO: Look into replacing the following dist_per_step divide with multiplying its inverse to save cycles. + + // Compute the number of steps in the prepped segment based on the approximate current rate. + // NOTE: The dist_per_step divide cancels out the INV_TIME_MULTIPLIER and converts the rate value to steps. + prep_segment->n_step = ceil(max(MINIMUM_STEP_RATE,st_prep_data->current_approx_rate)* + (ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND)/st_prep_data->dist_per_step); + // NOTE: Ensures it moves for very slow motions, but MINIMUM_STEP_RATE should always set this too. Perhaps + // a compile-time check to see if MINIMUM_STEP_RATE is set high enough is all that is needed. + prep_segment->n_step = max(prep_segment->n_step,MINIMUM_STEPS_PER_SEGMENT); + // NOTE: As long as the ACCELERATION_TICKS_PER_SECOND is valid, n_step should never exceed 255 and overflow. + // prep_segment->n_step = min(prep_segment->n_step,MAXIMUM_STEPS_PER_BLOCK); // Prevent unsigned int8 overflow. + + // Check if n_step exceeds steps remaining in planner block. If so, truncate. + if (prep_segment->n_step > st_prep_data->step_events_remaining) { + prep_segment->n_step = st_prep_data->step_events_remaining; + } + + // Check if n_step crosses decelerate point in block. If so, truncate to ensure the deceleration + // ramp counters are set correctly during execution. + if (st_prep_data->decelerate_after > 0) { + if (prep_segment->n_step > st_prep_data->decelerate_after) { + prep_segment->n_step = st_prep_data->decelerate_after; + } + } + + // Update stepper common data variables. + st_prep_data->decelerate_after -= prep_segment->n_step; + st_prep_data->step_events_remaining -= prep_segment->n_step; + + // Check for end of planner block + if ( st_prep_data->step_events_remaining == 0 ) { + + // TODO: When a feed hold ends, the step_events_remaining will also be zero, even though a block + // have partially been completed. We need to flag the stepper algorithm to indicate a stepper shutdown + // when complete, but not remove the planner block unless it truly is the end of the block (rare). + + // Set EOB bitflag so stepper algorithm discards the planner block after this segment completes. + prep_segment->flag |= SEGMENT_END_OF_BLOCK; + // Move planner pointer to next block and flag to load a new block for the next segment. + pl_prep_index = plan_next_block_index(pl_prep_index); + pl_prep_block = NULL; + } + + // New step segment completed. Increment segment buffer indices. + segment_buffer_head = segment_next_head; + if ( ++segment_next_head == SEGMENT_BUFFER_SIZE ) { segment_next_head = 0; } + +// long a = prep_segment->n_step; +// printInteger(a); +// printString(" "); +SPINDLE_ENABLE_PORT ^= 1<step_events_remaining*st_prep_data->mm_per_step; + if (st_prep_data->decelerate_after > 0) { *is_decelerating = false; } + else { *is_decelerating = true; } + + // Flag for new prep_block when st_prep_buffer() is called after the planner recomputes. + pl_partial_block_flag = true; + pl_prep_block = NULL; + } + return; +} diff --git a/stepper_old.c b/archive/stepper_old.c similarity index 100% rename from stepper_old.c rename to archive/stepper_old.c diff --git a/archive/stepper_time.c b/archive/stepper_time.c new file mode 100644 index 0000000..4b73098 --- /dev/null +++ b/archive/stepper_time.c @@ -0,0 +1,775 @@ +/* + stepper.c - stepper motor driver: executes motion plans using stepper motors + Part of Grbl + + Copyright (c) 2011-2013 Sungeun K. Jeon + Copyright (c) 2009-2011 Simen Svale Skogsrud + + 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 + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#include +#include "stepper.h" +#include "config.h" +#include "settings.h" +#include "planner.h" +#include "nuts_bolts.h" + +// Some useful constants +#define TICKS_PER_MICROSECOND (F_CPU/1000000) + +#define RAMP_NOOP_CRUISE 0 +#define RAMP_ACCEL 1 +#define RAMP_DECEL 2 + +#define LOAD_NOOP 0 +#define LOAD_SEGMENT 1 +#define LOAD_BLOCK 2 + +#define SEGMENT_NOOP 0 +#define SEGMENT_END_OF_BLOCK bit(0) +#define RAMP_CHANGE_ACCEL bit(1) +#define RAMP_CHANGE_DECEL bit(2) + +#define MINIMUM_STEPS_PER_SEGMENT 1 // Don't change + +#define SEGMENT_BUFFER_SIZE 6 + +#define DT_SEGMENT ACCELERATION_TICKS_PER_SECOND/ISR_TICKS_PER_SECOND + +// Stepper state variable. Contains running data and trapezoid variables. +typedef struct { + // Used by the bresenham line algorithm + int32_t counter_x, // Counter variables for the bresenham line tracer + counter_y, + counter_z; + + // Used by inverse time algorithm to track step rate + int32_t counter_dist; // Inverse time distance traveled since last step event + + uint8_t step_count; // Steps remaining in line segment motion + uint8_t phase_count; // Phase ticks remaining after line segment steps complete + + // Used by the stepper driver interrupt + uint8_t execute_step; // Flags step execution for each interrupt. + uint8_t step_pulse_time; // Step pulse reset time after step rise + uint8_t out_bits; // The next stepping-bits to be output + uint8_t load_flag; +} stepper_t; +static stepper_t st; + +// Stores stepper common data for executing steps in the segment buffer. Data can change mid-block when the +// planner updates the remaining block velocity profile with a more optimal plan or a feedrate override occurs. +// NOTE: Normally, this buffer is partially in-use, but, for the worst case scenario, it will never exceed +// the number of accessible stepper buffer segments (SEGMENT_BUFFER_SIZE-1). +typedef struct { + uint32_t dist_per_step; + float step_events_remaining; // Tracks step event count for the executing planner block + float accelerate_until; + float decelerate_after; + float current_rate; + float maximum_rate; + float exit_rate; + + float acceleration; + float step_per_mm; +} st_data_t; +static st_data_t segment_data[SEGMENT_BUFFER_SIZE-1]; + +// Primary stepper segment ring buffer. Contains small, short line segments for the stepper algorithm to execute, +// which are "checked-out" incrementally from the first block in the planner buffer. Once "checked-out", the steps +// in the segments buffer cannot be modified by the planner, where the remaining planner block steps still can. +typedef struct { + uint8_t n_step; // Number of step events to be executed for this segment + uint8_t n_phase_tick; + uint32_t dist_per_tick; + uint8_t st_data_index; // Stepper buffer common data index. Uses this information to execute this segment. + uint8_t flag; // Stepper algorithm bit-flag for special execution conditions. +} st_segment_t; +static st_segment_t segment_buffer[SEGMENT_BUFFER_SIZE]; + +// Step segment ring buffer indices +static volatile uint8_t segment_buffer_tail; +static volatile uint8_t segment_buffer_head; +static uint8_t segment_next_head; + +static volatile uint8_t busy; // Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though. +static plan_block_t *pl_current_block; // A pointer to the planner block currently being traced +static st_segment_t *st_current_segment; +static st_data_t *st_current_data; + +// Pointers for the step segment being prepped from the planner buffer. Accessed only by the +// main program. Pointers may be planning segments or planner blocks ahead of what being executed. +static plan_block_t *pl_prep_block; // Pointer to the planner block being prepped +static st_data_t *st_prep_data; // Pointer to the stepper common data being prepped +static uint8_t pl_prep_index; // Index of planner block being prepped +static uint8_t st_data_prep_index; // Index of stepper common data block being prepped +static uint8_t pl_partial_block_flag; // Flag indicating the planner has modified the prepped planner block + + +/* __________________________ + /| |\ _________________ ^ + / | | \ /| |\ | + / | | \ / | | \ s + / | | | | | \ p + / | | | | | \ e + +-----+------------------------+---+--+---------------+----+ e + | BLOCK 1 | BLOCK 2 | d + + time -----> + + The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta + until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after + after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as + +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. +*/ + +// Stepper state initialization. Cycle should only start if the st.cycle_start flag is +// enabled. Startup init and limits call this function but shouldn't start the cycle. +void st_wake_up() +{ + // Enable steppers by resetting the stepper disable port + if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { + STEPPERS_DISABLE_PORT |= (1<> 3); + // Enable stepper driver interrupt + st.execute_step = false; + st.load_flag = LOAD_BLOCK; + + TCNT2 = 0; // Clear Timer2 + TIMSK2 |= (1<n_step; + + // If the new segment starts a new planner block, initialize stepper variables and counters. + // NOTE: For new segments only, the step counters are not updated to ensure step phasing is continuous. + if (st.load_flag == LOAD_BLOCK) { + pl_current_block = plan_get_current_block(); // Should always be there. Stepper buffer handles this. + st_current_data = &segment_data[segment_buffer[segment_buffer_tail].st_data_index]; + + // Initialize direction bits for block. Set execute flag to set directions bits upon next ISR tick. + st.out_bits = pl_current_block->direction_bits ^ settings.invert_mask; + st.execute_step = true; + + // Initialize Bresenham line counters + st.counter_x = (pl_current_block->step_event_count >> 1); + st.counter_y = st.counter_x; + st.counter_z = st.counter_x; + + // Initialize inverse time, step rate data, and acceleration ramp counters + st.counter_dist = st_current_data->dist_per_step; // dist_per_step always greater than dist_per_tick. + } + + st.load_flag = LOAD_NOOP; // Segment motion loaded. Set no-operation flag to skip during execution. + + } else { + // Can't discard planner block here if a feed hold stops in middle of block. + st_go_idle(); + bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end + return; // Nothing to do but exit. + } + + } + + // Iterate inverse time counter. Triggers each Bresenham step event. + st.counter_dist -= st_current_segment->dist_per_tick; + + // Execute Bresenham step event, when it's time to do so. + if (st.counter_dist < 0) { + if (st.step_count > 0) { // Block phase correction from executing step. + st.counter_dist += st_current_data->dist_per_step; // Reload inverse time counter + + st.out_bits = pl_current_block->direction_bits; // Reset out_bits and reload direction bits + st.execute_step = true; + + // Execute step displacement profile by Bresenham line algorithm + st.counter_x -= pl_current_block->steps[X_AXIS]; + if (st.counter_x < 0) { + st.out_bits |= (1<step_event_count; + if (st.out_bits & (1<steps[Y_AXIS]; + if (st.counter_y < 0) { + st.out_bits |= (1<step_event_count; + if (st.out_bits & (1<steps[Z_AXIS]; + if (st.counter_z < 0) { + st.out_bits |= (1<step_event_count; + if (st.out_bits & (1<flag & SEGMENT_END_OF_BLOCK) { + plan_discard_current_block(); + st.load_flag = LOAD_BLOCK; + } else { + st.load_flag = LOAD_SEGMENT; + } + + // Discard current segment by advancing buffer tail index + if ( ++segment_buffer_tail == SEGMENT_BUFFER_SIZE) { segment_buffer_tail = 0; } + } + st.phase_count--; + } + + + busy = false; +// SPINDLE_ENABLE_PORT ^= 1<step_events_remaining); +// st.ramp_type = RAMP_ACCEL; +// st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK/2; +// st.ramp_rate = 0; +// sys.state = STATE_QUEUED; +// } else { +// sys.state = STATE_IDLE; +// } + sys.state = STATE_IDLE; + +} + + +/* Prepares step segment buffer. Continuously called from main program. + + The segment buffer is an intermediary buffer interface between the execution of steps + by the stepper algorithm and the velocity profiles generated by the planner. The stepper + algorithm only executes steps within the segment buffer and is filled by the main program + when steps are "checked-out" from the first block in the planner buffer. This keeps the + step execution and planning optimization processes atomic and protected from each other. + The number of steps "checked-out" from the planner buffer and the number of segments in + the segment buffer is sized and computed such that no operation in the main program takes + longer than the time it takes the stepper algorithm to empty it before refilling it. + Currently, the segment buffer conservatively holds roughly up to 40-60 msec of steps. + + NOTE: The segment buffer executes a set number of steps over an approximate time period. + If we try to execute over a fixed time period, it is difficult to guarantee or predict + how many steps will execute over it, especially when the step pulse phasing between the + neighboring segments must also be kept consistent. Meaning that, if the last segment step + pulses right before a segment end, the next segment must delay its first pulse so that the + step pulses are consistently spaced apart over time to keep the step pulse train nice and + smooth. Keeping track of phasing and ensuring that the exact number of steps are executed + as defined by the planner block, the related computational overhead can get quickly and + prohibitively expensive, especially in real-time. + Since the stepper algorithm automatically takes care of the step pulse phasing with + its ramp and inverse time counters by retaining the count remainders, we don't have to + explicitly and expensively track and synchronize the exact number of steps, time, and + phasing of steps. All we need to do is approximate the number of steps in each segment + such that the segment buffer has enough execution time for the main program to do what + it needs to do and refill it when it comes back. In other words, we just need to compute + a cheap approximation of the current velocity and the number of steps over it. +*/ + +/* + TODO: Figure out how to enforce a deceleration when a feedrate override is reduced. + The problem is that when an override is reduced, the planner may not plan back to + the current rate. Meaning that the velocity profiles for certain conditions no longer + are trapezoidal or triangular. For example, if the current block is cruising at a + nominal rate and the feedrate override is reduced, the new nominal rate will now be + lower. The velocity profile must first decelerate to the new nominal rate and then + follow on the new plan. So the remaining velocity profile will have a decelerate, + cruise, and another decelerate. + Another issue is whether or not a feedrate override reduction causes a deceleration + that acts over several planner blocks. For example, say that the plan is already + heavily decelerating throughout it, reducing the feedrate will not do much to it. So, + how do we determine when to resume the new plan? How many blocks do we have to wait + until the new plan intersects with the deceleration curve? One plus though, the + deceleration will never be more than the number of blocks in the entire planner buffer, + but it theoretically can be equal to it when all planner blocks are decelerating already. +*/ +void st_prep_buffer() +{ + if (sys.state == STATE_QUEUED) { return; } // Block until a motion state is issued + while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer. + + // Initialize new segment + st_segment_t *prep_segment = &segment_buffer[segment_buffer_head]; + prep_segment->flag = SEGMENT_NOOP; + + // ----------------------------------------------------------------------------------- + // Determine if we need to load a new planner block. If so, prepare step data. + if (pl_prep_block == NULL) { + pl_prep_block = plan_get_block_by_index(pl_prep_index); // Query planner for a queued block + if (pl_prep_block == NULL) { return; } // No planner blocks. Exit. + SPINDLE_ENABLE_PORT ^= 1<step_events_remaining = last_st_prep_data->step_events_remaining; + st_prep_data->dist_per_step = last_st_prep_data->dist_per_step; + st_prep_data->step_per_mm = last_st_prep_data->step_per_mm; + st_prep_data->acceleration = last_st_prep_data->acceleration; + + pl_partial_block_flag = false; // Reset flag + + } else { + + // Prepare commonly shared planner block data for the ensuing segment buffer moves ad-hoc, since + // the planner buffer can dynamically change the velocity profile data as blocks are added. + st_prep_data = &segment_data[st_data_prep_index]; + + // Initialize planner block step data + st_prep_data->step_events_remaining = pl_prep_block->step_event_count; + st_prep_data->step_per_mm = pl_prep_block->step_event_count/pl_prep_block->millimeters; + st_prep_data->dist_per_step = ceil(INV_TIME_MULTIPLIER/st_prep_data->step_per_mm); // (mult*mm/step) + st_prep_data->acceleration = st_prep_data->step_per_mm*pl_prep_block->acceleration; + + } + + // Convert planner entry speed to stepper initial rate. + st_prep_data->current_rate = st_prep_data->step_per_mm*sqrt(pl_prep_block->entry_speed_sqr); + + // Determine current block exit speed + plan_block_t *pl_next_block = plan_get_block_by_index(plan_next_block_index(pl_prep_index)); + float exit_speed_sqr; + if (pl_next_block != NULL) { + exit_speed_sqr = pl_next_block->entry_speed_sqr; + st_prep_data->exit_rate = st_prep_data->step_per_mm*sqrt(exit_speed_sqr); + } else { + exit_speed_sqr = 0.0; // End of planner buffer. Zero speed. + st_prep_data->exit_rate = 0.0; + } + + // Determine velocity profile based on the 7 possible types: Cruise-only, cruise-deceleration, + // acceleration-cruise, acceleration-only, deceleration-only, trapezoid, and triangle. + st_prep_data->accelerate_until = pl_prep_block->millimeters; + if (pl_prep_block->entry_speed_sqr == pl_prep_block->nominal_speed_sqr) { + st_prep_data->maximum_rate = sqrt(pl_prep_block->nominal_speed_sqr); + st_prep_data->accelerate_until = pl_prep_block->millimeters; + if (exit_speed_sqr == pl_prep_block->nominal_speed_sqr) { // Cruise-only type + st_prep_data->decelerate_after = 0.0; + } else { // Cruise-deceleration type + st_prep_data->decelerate_after = (pl_prep_block->nominal_speed_sqr-exit_speed_sqr)/(2*pl_prep_block->acceleration); + } + } else if (exit_speed_sqr == pl_prep_block->nominal_speed_sqr) { + // Acceleration-cruise type + st_prep_data->maximum_rate = sqrt(pl_prep_block->nominal_speed_sqr); + st_prep_data->decelerate_after = 0.0; + st_prep_data->accelerate_until -= (pl_prep_block->nominal_speed_sqr-pl_prep_block->entry_speed_sqr)/(2*pl_prep_block->acceleration); + } else { + float intersection_dist = 0.5*( pl_prep_block->millimeters + (pl_prep_block->entry_speed_sqr + - exit_speed_sqr)/(2*pl_prep_block->acceleration) ); + if (intersection_dist > 0.0) { + if (intersection_dist < pl_prep_block->millimeters) { // Either trapezoid or triangle types + st_prep_data->decelerate_after = (pl_prep_block->nominal_speed_sqr-exit_speed_sqr)/(2*pl_prep_block->acceleration); + if (st_prep_data->decelerate_after < intersection_dist) { // Trapezoid type + st_prep_data->maximum_rate = sqrt(pl_prep_block->nominal_speed_sqr); + st_prep_data->accelerate_until -= (pl_prep_block->nominal_speed_sqr-pl_prep_block->entry_speed_sqr)/(2*pl_prep_block->acceleration); + } else { // Triangle type + st_prep_data->decelerate_after = intersection_dist; + st_prep_data->maximum_rate = sqrt(2*pl_prep_block->acceleration*st_prep_data->decelerate_after+exit_speed_sqr); + st_prep_data->accelerate_until -= st_prep_data->decelerate_after; + } + } else { // Deceleration-only type + st_prep_data->maximum_rate = sqrt(pl_prep_block->entry_speed_sqr); + st_prep_data->decelerate_after = pl_prep_block->millimeters; + } + } else { // Acceleration-only type + st_prep_data->maximum_rate = sqrt(exit_speed_sqr); + st_prep_data->decelerate_after = 0.0; + st_prep_data->accelerate_until = 0.0; + } + } + + // Determine block velocity profile parameters +// st_prep_data->accelerate_until = (pl_prep_block->nominal_speed_sqr-pl_prep_block->entry_speed_sqr)/(2*pl_prep_block->acceleration); +// st_prep_data->decelerate_after = (pl_prep_block->nominal_speed_sqr-exit_speed_sqr)/(2*pl_prep_block->acceleration); +// +// // Determine if velocity profile is a triangle or trapezoid. +// if (pl_prep_block->millimeters < st_prep_data->accelerate_until+st_prep_data->decelerate_after) { +// st_prep_data->decelerate_after = 0.5*( pl_prep_block->millimeters + (pl_prep_block->entry_speed_sqr +// - exit_speed_sqr)/(2*pl_prep_block->acceleration) ); +// st_prep_data->accelerate_until = pl_prep_block->millimeters-st_prep_data->decelerate_after; +// st_prep_data->maximum_speed = sqrt(2*pl_prep_block->acceleration*st_prep_data->decelerate_after+exit_speed_sqr); +// } else { +// st_prep_data->accelerate_until = pl_prep_block->millimeters-st_prep_data->accelerate_until; +// st_prep_data->maximum_speed = sqrt(pl_prep_block->nominal_speed_sqr); +// } + + // Convert velocity profile parameters in terms of steps. + st_prep_data->maximum_rate *= st_prep_data->step_per_mm; + st_prep_data->accelerate_until *= st_prep_data->step_per_mm; + st_prep_data->decelerate_after *= st_prep_data->step_per_mm; + + } + + // Set new segment to point to the current segment data block. + prep_segment->st_data_index = st_data_prep_index; + + // ----------------------------------------------------------------------------------- + // Initialize segment execute distance. Attempt to create a full segment over DT_SEGMENT. + // NOTE: Computed in terms of steps and seconds to prevent numerical round-off issues. + + float steps_remaining = st_prep_data->step_events_remaining; + float dt = DT_SEGMENT; + if (steps_remaining > st_prep_data->accelerate_until) { // Acceleration ramp + steps_remaining -= st_prep_data->current_rate*DT_SEGMENT + + st_prep_data->acceleration*(0.5*DT_SEGMENT*DT_SEGMENT); + if (steps_remaining < st_prep_data->accelerate_until) { // **Incomplete** Acceleration ramp end. + // Acceleration-cruise, acceleration-deceleration ramp junction, or end of block + steps_remaining = st_prep_data->accelerate_until; + dt = 2*(st_prep_data->step_events_remaining-steps_remaining)/ + (st_prep_data->current_rate+st_prep_data->maximum_rate); + st_prep_data->current_rate = st_prep_data->maximum_rate; + } else { // **Complete** Acceleration only. + st_prep_data->current_rate += st_prep_data->acceleration*DT_SEGMENT; + } + } else if (steps_remaining <= st_prep_data->decelerate_after) { // Deceleration ramp + steps_remaining -= st_prep_data->current_rate*DT_SEGMENT + - st_prep_data->acceleration*(0.5*DT_SEGMENT*DT_SEGMENT); + if (steps_remaining > 0) { // **Complete** Deceleration only. + st_prep_data->current_rate -= st_prep_data->acceleration*DT_SEGMENT; + } else { // **Complete* End of block. + dt = 2*st_prep_data->step_events_remaining/(st_prep_data->current_rate+st_prep_data->exit_rate); + steps_remaining = 0; + // st_prep_data->current_speed = st_prep_data->exit_speed; + } + } else { // Cruising profile + steps_remaining -= st_prep_data->maximum_rate*DT_SEGMENT; + if (steps_remaining < st_prep_data->decelerate_after) { // **Incomplete** End of cruise. + steps_remaining = st_prep_data->decelerate_after; + dt = (st_prep_data->step_events_remaining-steps_remaining)/st_prep_data->maximum_rate; + } // Otherwise **Complete** Cruising only. + } + + // ----------------------------------------------------------------------------------- + // If segment is incomplete, attempt to fill the remainder. + // NOTE: Segment remainder always spans a cruise and/or a deceleration ramp. + + if (dt < DT_SEGMENT) { + if (steps_remaining > 0) { // Skip if end of block. + float last_steps_remaining; + + // Fill incomplete segment with an acceleration junction. + if (steps_remaining > st_prep_data->decelerate_after) { // Cruising profile + last_steps_remaining = steps_remaining; + steps_remaining -= st_prep_data->current_rate*(DT_SEGMENT-dt); + if (steps_remaining < st_prep_data->decelerate_after) { // **Incomplete** + steps_remaining = st_prep_data->decelerate_after; + dt += (last_steps_remaining-steps_remaining)/st_prep_data->maximum_rate; + // current_speed = maximum_speed; + } else { // **Complete** Segment filled. + dt = DT_SEGMENT; + } + } + + // Fill incomplete segment with a deceleration junction. + if (steps_remaining > 0) { + if (steps_remaining <= st_prep_data->decelerate_after) { // Deceleration ramp + last_steps_remaining = steps_remaining; + float dt_remainder = DT_SEGMENT-dt; + steps_remaining -= dt_remainder*(st_prep_data->current_rate + - 0.5*st_prep_data->acceleration*dt_remainder); + if (steps_remaining > 0) { // **Complete** Segment filled. + st_prep_data->current_rate -= st_prep_data->acceleration*dt_remainder; + dt = DT_SEGMENT; + } else { // **Complete** End of block. + steps_remaining = 0; + dt += (2*last_steps_remaining/(st_prep_data->current_rate+st_prep_data->exit_rate)); + // st_prep_data->current_speed = st_prep_data->exit_speed; + } + } + } + + } + } + + // ----------------------------------------------------------------------------------- + // Compute segment step rate, steps to execute, and step phase correction parameters. + // NOTE: + + // !!! PROBLEM. Step events remaining in floating point can limit the number of steps + // we can accurately track, since floats have 8 significant digits. However, this only + // becomes a problem if there are more than 10,000,000, which translates to a CNC machine + // with 800 step/mm and 10 meters of axis travel. + + prep_segment->dist_per_tick = ceil((st_prep_data->step_events_remaining-steps_remaining) + /dt*(INV_TIME_MULTIPLIER/ISR_TICKS_PER_SECOND)); // (mult*mm/isr_tic) + + if (steps_remaining > 0) { + + // Compute number of steps to execute and segment step phase correction. + prep_segment->n_step = ceil(st_prep_data->step_events_remaining)-ceil(steps_remaining); + prep_segment->n_phase_tick = ceil((ceil(steps_remaining)-steps_remaining)*st_prep_data->dist_per_step); + + } else { // End of block. Finish it out. + + // Set to execute the remaining steps and no phase correction upon finishing the block. + prep_segment->n_step = ceil(st_prep_data->step_events_remaining); + prep_segment->n_phase_tick = 0; + + // Move planner pointer to next block and flag to load a new block for the next segment. + pl_prep_index = plan_next_block_index(pl_prep_index); + pl_prep_block = NULL; + prep_segment->flag |= SEGMENT_END_OF_BLOCK; + } + + // Update step execution variables + st_prep_data->step_events_remaining = steps_remaining; + + // Ensure the initial step rate exceeds the MINIMUM_STEP_RATE. + // TODO: Use config.h error checking to do this. Otherwise, counters get screwy. + + // New step segment initialization completed. Increment segment buffer indices. + segment_buffer_head = segment_next_head; + if ( ++segment_next_head == SEGMENT_BUFFER_SIZE ) { segment_next_head = 0; } + SPINDLE_ENABLE_PORT ^= 1<step_events_remaining/st_prep_data->step_per_mm; + if (st_prep_data->step_events_remaining < st_prep_data->decelerate_after) { *is_decelerating = true; } + else { *is_decelerating = false; } + + // Flag for new prep_block when st_prep_buffer() is called after the planner recomputes. + pl_partial_block_flag = true; + pl_prep_block = NULL; + } + return; +} diff --git a/archive/stepper_time_archive.c b/archive/stepper_time_archive.c new file mode 100644 index 0000000..dac918f --- /dev/null +++ b/archive/stepper_time_archive.c @@ -0,0 +1,823 @@ +/* + stepper.c - stepper motor driver: executes motion plans using stepper motors + Part of Grbl + + Copyright (c) 2011-2013 Sungeun K. Jeon + Copyright (c) 2009-2011 Simen Svale Skogsrud + + 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 + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#include +#include "stepper.h" +#include "config.h" +#include "settings.h" +#include "planner.h" +#include "nuts_bolts.h" + +// Some useful constants +#define TICKS_PER_MICROSECOND (F_CPU/1000000) + +#define RAMP_ACCEL 0 +#define RAMP_CRUISE 1 +#define RAMP_DECEL 2 + +#define LOAD_NOOP 0 +#define LOAD_SEGMENT 1 +#define LOAD_BLOCK 2 + +#define SEGMENT_NOOP 0 +#define SEGMENT_END_OF_BLOCK bit(0) +#define RAMP_CHANGE_ACCEL bit(1) +#define RAMP_CHANGE_DECEL bit(2) + +#define SEGMENT_BUFFER_SIZE 6 + +#define DT_SEGMENT (1.0/(ACCELERATION_TICKS_PER_SECOND*60.0)) + +// Stores the planner block Bresenham algorithm execution data for the segments in the segment +// buffer. Normally, this buffer is partially in-use, but, for the worst case scenario, it will +// never exceed the number of accessible stepper buffer segments (SEGMENT_BUFFER_SIZE-1). +// NOTE: This data is copied from the prepped planner blocks so that the planner blocks may be +// discarded when entirely consumed and completed by the segment buffer. +typedef struct { + uint8_t direction_bits; + int32_t steps[N_AXIS]; + int32_t step_event_count; +} st_block_t; +static st_block_t st_block_buffer[SEGMENT_BUFFER_SIZE-1]; +// TODO: Directly adjust this parameters to stop motion of individual axes for the homing cycle. +// But this may require this to be volatile if it is controlled by an interrupt. + +// Primary stepper segment ring buffer. Contains small, short line segments for the stepper +// algorithm to execute, which are "checked-out" incrementally from the first block in the +// planner buffer. Once "checked-out", the steps in the segments buffer cannot be modified by +// the planner, where the remaining planner block steps still can. +typedef struct { + uint8_t n_step; // Number of step events to be executed for this segment + uint8_t st_block_index; // Stepper block data index. Uses this information to execute this segment. + int32_t phase_dist; + int32_t dist_per_tick; +} segment_t; +static segment_t segment_buffer[SEGMENT_BUFFER_SIZE]; + +// Stepper state variable. Contains running data and trapezoid variables. +typedef struct { + // Used by the bresenham line algorithm + int32_t counter_x, // Counter variables for the bresenham line tracer + counter_y, + counter_z; + + // Used by inverse time algorithm to track step rate + int32_t counter_dist; // Inverse time distance traveled since last step event + + // Used by the stepper driver interrupt + uint8_t execute_step; // Flags step execution for each interrupt. + uint8_t step_pulse_time; // Step pulse reset time after step rise + uint8_t out_bits; // The next stepping-bits to be output + + uint8_t step_count; // Steps remaining in line segment motion + uint8_t exec_block_index; // Tracks the current st_block index. Change indicates new block. + st_block_t *exec_block; // Pointer to the block data for the segment being executed + segment_t *exec_segment; // Pointer to the segment being executed +} stepper_t; +static stepper_t st; + +// Step segment ring buffer indices +static volatile uint8_t segment_buffer_tail; +static volatile uint8_t segment_buffer_head; +static uint8_t segment_next_head; + +static volatile uint8_t busy; // Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though. + +// Pointers for the step segment being prepped from the planner buffer. Accessed only by the +// main program. Pointers may be planning segments or planner blocks ahead of what being executed. +static plan_block_t *pl_block; // Pointer to the planner block being prepped +static st_block_t *st_prep_block; // Pointer to the stepper block data being prepped + +typedef struct { + uint8_t st_block_index; // Index of stepper common data block being prepped + uint8_t partial_block_flag; // Flag indicating the planner has modified the prepped planner block + + float step_per_mm; + float step_events_remaining; // Tracks step event count for the executing planner block +// int32_t step_events_remaining; + float step_remainder; + + uint8_t ramp_type; + float current_speed; + float maximum_speed; + float exit_speed; + float accelerate_until; + float decelerate_after; +} st_prep_t; +static st_prep_t prep; + + +/* __________________________ + /| |\ _________________ ^ + / | | \ /| |\ | + / | | \ / | | \ s + / | | | | | \ p + / | | | | | \ e + +-----+------------------------+---+--+---------------+----+ e + | BLOCK 1 | BLOCK 2 | d + + time -----> + + The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta + until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after + after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as + +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. +*/ + +// Stepper state initialization. Cycle should only start if the st.cycle_start flag is +// enabled. Startup init and limits call this function but shouldn't start the cycle. +void st_wake_up() +{ + // Enable steppers by resetting the stepper disable port + if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { + STEPPERS_DISABLE_PORT |= (1<> 3); + + // Enable stepper driver interrupt + TCNT2 = 0; // Clear Timer2 + TIMSK2 |= (1<n_step; + + // If the new segment starts a new planner block, initialize stepper variables and counters. + // NOTE: When the segment data index changes, this indicates a new planner block. + if ( st.exec_block_index != st.exec_segment->st_block_index ) { + st.exec_block_index = st.exec_segment->st_block_index; + st.exec_block = &st_block_buffer[st.exec_block_index]; + + // Initialize direction bits for block. Set execute flag to set directions bits upon next ISR tick. + st.out_bits = st.exec_block->direction_bits ^ settings.invert_mask; + st.execute_step = true; + + // Initialize Bresenham line counters + st.counter_x = (st.exec_block->step_event_count >> 1); + st.counter_y = st.counter_x; + st.counter_z = st.counter_x; + + // Initialize inverse time, step rate data, and acceleration ramp counters + st.counter_dist = INV_TIME_MULTIPLIER; // dist_per_step always greater than dist_per_tick. + } + + } else { + // Segment buffer empty. Shutdown. + st_go_idle(); + bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end + return; // Nothing to do but exit. + } + + } + + // Iterate inverse time counter. Triggers each Bresenham step event. + st.counter_dist -= st.exec_segment->dist_per_tick; + + // Execute Bresenham step event, when it's time to do so. + if (st.counter_dist < 0) { + if (st.step_count != 0) { // Block phase correction from executing step. + st.counter_dist += INV_TIME_MULTIPLIER; // Reload inverse time counter + st.step_count--; // Decrement step events count + + // Execute step displacement profile by Bresenham line algorithm + st.execute_step = true; + st.out_bits = st.exec_block->direction_bits; // Reset out_bits and reload direction bits + st.counter_x -= st.exec_block->steps[X_AXIS]; + if (st.counter_x < 0) { + st.out_bits |= (1<step_event_count; + if (st.out_bits & (1<steps[Y_AXIS]; + if (st.counter_y < 0) { + st.out_bits |= (1<step_event_count; + if (st.out_bits & (1<steps[Z_AXIS]; + if (st.counter_z < 0) { + st.out_bits |= (1<step_event_count; + if (st.out_bits & (1<phase_dist > st.counter_dist) { + // Segment is complete. Discard current segment and advance segment indexing. + st.exec_segment = NULL; + if ( ++segment_buffer_tail == SEGMENT_BUFFER_SIZE) { segment_buffer_tail = 0; } + } + } + + busy = false; +// SPINDLE_ENABLE_PORT ^= 1<step_events_remaining); +// st.ramp_type = RAMP_ACCEL; +// st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK/2; +// st.ramp_rate = 0; +// sys.state = STATE_QUEUED; +// } else { +// sys.state = STATE_IDLE; +// } + sys.state = STATE_IDLE; + +} + + +/* Prepares step segment buffer. Continuously called from main program. + + The segment buffer is an intermediary buffer interface between the execution of steps + by the stepper algorithm and the velocity profiles generated by the planner. The stepper + algorithm only executes steps within the segment buffer and is filled by the main program + when steps are "checked-out" from the first block in the planner buffer. This keeps the + step execution and planning optimization processes atomic and protected from each other. + The number of steps "checked-out" from the planner buffer and the number of segments in + the segment buffer is sized and computed such that no operation in the main program takes + longer than the time it takes the stepper algorithm to empty it before refilling it. + Currently, the segment buffer conservatively holds roughly up to 40-60 msec of steps. + + NOTE: The segment buffer executes a set number of steps over an approximate time period. + If we try to execute over a fixed time period, it is difficult to guarantee or predict + how many steps will execute over it, especially when the step pulse phasing between the + neighboring segments must also be kept consistent. Meaning that, if the last segment step + pulses right before a segment end, the next segment must delay its first pulse so that the + step pulses are consistently spaced apart over time to keep the step pulse train nice and + smooth. Keeping track of phasing and ensuring that the exact number of steps are executed + as defined by the planner block, the related computational overhead can get quickly and + prohibitively expensive, especially in real-time. + Since the stepper algorithm automatically takes care of the step pulse phasing with + its ramp and inverse time counters by retaining the count remainders, we don't have to + explicitly and expensively track and synchronize the exact number of steps, time, and + phasing of steps. All we need to do is approximate the number of steps in each segment + such that the segment buffer has enough execution time for the main program to do what + it needs to do and refill it when it comes back. In other words, we just need to compute + a cheap approximation of the current velocity and the number of steps over it. +*/ + +/* + TODO: Figure out how to enforce a deceleration when a feedrate override is reduced. + The problem is that when an override is reduced, the planner may not plan back to + the current rate. Meaning that the velocity profiles for certain conditions no longer + are trapezoidal or triangular. For example, if the current block is cruising at a + nominal rate and the feedrate override is reduced, the new nominal rate will now be + lower. The velocity profile must first decelerate to the new nominal rate and then + follow on the new plan. So the remaining velocity profile will have a decelerate, + cruise, and another decelerate. + Another issue is whether or not a feedrate override reduction causes a deceleration + that acts over several planner blocks. For example, say that the plan is already + heavily decelerating throughout it, reducing the feedrate will not do much to it. So, + how do we determine when to resume the new plan? How many blocks do we have to wait + until the new plan intersects with the deceleration curve? One plus though, the + deceleration will never be more than the number of blocks in the entire planner buffer, + but it theoretically can be equal to it when all planner blocks are decelerating already. +*/ +void st_prep_buffer() +{ + if (sys.state == STATE_QUEUED) { return; } // Block until a motion state is issued + while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer. + + + // ----------------------------------------------------------------------------------- + // Determine if we need to load a new planner block. If so, prepare step data. + if (pl_block == NULL) { + pl_block = plan_get_current_block(); // Query planner for a queued block + if (pl_block == NULL) { return; } // No planner blocks. Exit. + +// SPINDLE_ENABLE_PORT ^= 1<steps[X_AXIS] = pl_block->steps[X_AXIS]; + st_prep_block->steps[Y_AXIS] = pl_block->steps[Y_AXIS]; + st_prep_block->steps[Z_AXIS] = pl_block->steps[Z_AXIS]; + st_prep_block->direction_bits = pl_block->direction_bits; + st_prep_block->step_event_count = pl_block->step_event_count; + + // Initialize planner block step count, unit distance data, and remainder tracker. + prep.step_per_mm = ((float)st_prep_block->step_event_count)/pl_block->millimeters; + prep.step_events_remaining = st_prep_block->step_event_count; + prep.step_remainder = 0.0; + } + + // Compute the prepped planner block velocity profile to be traced by stepper algorithm. + prep.current_speed = sqrt(pl_block->entry_speed_sqr); + prep.exit_speed = plan_get_exec_block_exit_speed(); + + // Determine velocity profile based on the 7 possible types: Cruise-only, cruise-deceleration, + // acceleration-cruise, acceleration-only, deceleration-only, full-trapezoid, and triangle. + prep.ramp_type = RAMP_ACCEL; + float exit_speed_sqr = prep.exit_speed*prep.exit_speed; + float inv_2_accel = 0.5/pl_block->acceleration; + float intersection_dist = + 0.5*(pl_block->millimeters+inv_2_accel*(pl_block->entry_speed_sqr-exit_speed_sqr)); + if (intersection_dist > 0.0) { + if (intersection_dist < pl_block->millimeters) { // Either trapezoid or triangle types + // NOTE: For acceleration-cruise trapezoid, following calculation will be 0.0. + prep.decelerate_after = inv_2_accel*(pl_block->nominal_speed_sqr-exit_speed_sqr); + if (prep.decelerate_after < intersection_dist) { // Trapezoid type + prep.maximum_speed = sqrt(pl_block->nominal_speed_sqr); + if (pl_block->entry_speed_sqr == pl_block->nominal_speed_sqr) { + // Cruise-deceleration or cruise-only type. + prep.ramp_type = RAMP_CRUISE; + prep.accelerate_until = pl_block->millimeters; + } else { + // Full-trapezoid or acceleration-cruise types + prep.accelerate_until = + pl_block->millimeters-inv_2_accel*(pl_block->nominal_speed_sqr-pl_block->entry_speed_sqr); + } + } else { // Triangle type + prep.accelerate_until = intersection_dist; + prep.decelerate_after = intersection_dist; + prep.maximum_speed = sqrt(2.0*pl_block->acceleration*intersection_dist+exit_speed_sqr); + } + } else { // Deceleration-only type + prep.ramp_type = RAMP_DECEL; + prep.maximum_speed = prep.current_speed; + prep.accelerate_until = pl_block->millimeters; + prep.decelerate_after = pl_block->millimeters; + } + } else { // Acceleration-only type + prep.maximum_speed = prep.exit_speed; + prep.accelerate_until = 0.0; + prep.decelerate_after = 0.0; + } + + } + + // Initialize new segment + segment_t *prep_segment = &segment_buffer[segment_buffer_head]; + + // Set new segment to point to the current segment data block. + prep_segment->st_block_index = prep.st_block_index; + + /* ----------------------------------------------------------------------------------- + Compute the average velocity of this new segment by determining the total distance + traveled over the segment time DT_SEGMENT. This section attempts to create a full + segment based on the current ramp conditions. If the segment is incomplete and + terminates upon a ramp change, the next section will attempt to fill the remaining + segment execution time. However, if an incomplete segment terminates at the end of + the planner block, the segment execution time is less than DT_SEGMENT and the new + segment will execute over this truncated execution time. + */ + float dt = 0.0; + float mm_remaining = pl_block->millimeters; + float dt_var = DT_SEGMENT; + float mm_var; + do { + switch (prep.ramp_type) { + case RAMP_ACCEL: + // NOTE: Acceleration ramp always computes during first loop only. + mm_remaining -= DT_SEGMENT*(prep.current_speed + pl_block->acceleration*(0.5*DT_SEGMENT)); + if (mm_remaining < prep.accelerate_until) { // End of acceleration ramp. + // Acceleration-cruise, acceleration-deceleration ramp junction, or end of block. + mm_remaining = prep.accelerate_until; // NOTE: 0.0 at EOB + dt_var = 2.0*(pl_block->millimeters-mm_remaining)/(prep.current_speed+prep.maximum_speed); + if (mm_remaining == prep.decelerate_after) { prep.ramp_type = RAMP_DECEL; } + else { prep.ramp_type = RAMP_CRUISE; } + prep.current_speed = prep.maximum_speed; + } else { // Acceleration only. + prep.current_speed += pl_block->acceleration*dt_var; + } + break; + case RAMP_CRUISE: + // NOTE: mm_var used to retain the last mm_remaining for incomplete segment dt_var calculations. + mm_var = mm_remaining - prep.maximum_speed*dt_var; + if (mm_var < prep.decelerate_after) { // End of cruise. + // Cruise-deceleration junction or end of block. + dt_var = (mm_remaining - prep.decelerate_after)/prep.maximum_speed; + mm_remaining = prep.decelerate_after; // NOTE: 0.0 at EOB + prep.ramp_type = RAMP_DECEL; + } else { // Cruising only. + mm_remaining = mm_var; + } + break; + default: // case RAMP_DECEL: + // NOTE: mm_var used to catch negative decelerate distance values near zero speed. + mm_var = dt_var*(prep.current_speed - 0.5*pl_block->acceleration*dt_var); + if ((mm_var > 0.0) && (mm_var < pl_block->millimeters)) { // Deceleration only. + prep.current_speed -= pl_block->acceleration*dt_var; + // Check for near-zero speed and prevent divide by zero in rare scenarios. + if (prep.current_speed <= prep.exit_speed) { mm_remaining = 0.0; } + else { mm_remaining -= mm_var; } + } else { // End of block. + dt_var = 2.0*mm_remaining/(prep.current_speed+prep.exit_speed); + mm_remaining = 0.0; + // prep.current_speed = prep.exit_speed; + } + } + dt += dt_var; + if (dt < DT_SEGMENT) { dt_var = DT_SEGMENT - dt; } // **Incomplete** At ramp junction. + else { break; } // **Complete** Exit loop. Segment execution time maxed. + } while ( mm_remaining > 0.0 ); // **Complete** Exit loop. End of planner block. + + /* + float mm_remaining; + float dt = DT_SEGMENT; + if (pl_block->millimeters > prep.accelerate_until) { // [Acceleration Ramp] + mm_remaining = pl_block->millimeters - DT_SEGMENT*(prep.current_speed + pl_block->acceleration*(0.5*DT_SEGMENT)); + if (mm_remaining < prep.accelerate_until) { // **Incomplete** Acceleration ramp end. + // Acceleration-cruise, acceleration-deceleration ramp junction, or end of block. + mm_remaining = prep.accelerate_until; // NOTE: 0.0 at EOB + dt = 2.0*(pl_block->millimeters-mm_remaining)/(prep.current_speed+prep.maximum_speed); + prep.current_speed = prep.maximum_speed; + } else { // **Complete** Acceleration only. + prep.current_speed += pl_block->acceleration*DT_SEGMENT; + prep.current_speed = min(prep.maximum_speed,prep.current_speed); + } + } else if (pl_block->millimeters > prep.decelerate_after) { // [No Ramp. Cruising] + mm_remaining = pl_block->millimeters - prep.maximum_speed*DT_SEGMENT; + if (mm_remaining < prep.decelerate_after) { // **Incomplete** End of cruise. + // Cruise-deceleration junction or end of block. + mm_remaining = prep.decelerate_after; // NOTE: 0.0 at EOB + dt = (pl_block->millimeters-mm_remaining)/prep.maximum_speed; + } // Otherwise **Complete** Cruising only. + } else { // [Deceleration Ramp] + mm_remaining = DT_SEGMENT*(prep.current_speed - 0.5*pl_block->acceleration*DT_SEGMENT); + if ((mm_remaining > 0.0) && (mm_remaining < pl_block->millimeters)) { // **Complete** Deceleration only. + prep.current_speed -= pl_block->acceleration*DT_SEGMENT; + if (prep.current_speed <= prep.exit_speed) { // Round off error fix. Prevents divide by zero. + mm_remaining = 0.0; + } else { + mm_remaining = pl_block->millimeters - mm_remaining; + } + } else { // **Complete** End of block. + mm_remaining = 0.0; + dt = 2.0*pl_block->millimeters/(prep.current_speed+prep.exit_speed); + // prep.current_speed = prep.exit_speed; + } + } + + + /* ----------------------------------------------------------------------------------- + If segment is incomplete, attempt to fill the remaining segment execution time. + NOTE: Segment remainder always spans a cruise and/or a deceleration ramp. + + float partial_mm, dt_remainder; + if ((dt < DT_SEGMENT) && (mm_remaining > 0.0)) { + dt_remainder = DT_SEGMENT-dt; + + // Attempt to fill incomplete segment with cruising profile. + if (mm_remaining > prep.decelerate_after) { // Cruising profile + partial_mm = mm_remaining - prep.current_speed*dt_remainder; + if (partial_mm < prep.decelerate_after) { // **Incomplete** + dt += (mm_remaining-prep.decelerate_after)/prep.maximum_speed; + mm_remaining = prep.decelerate_after; + // current_speed = maximum_speed; + } else { // **Complete** Segment filled. + mm_remaining = partial_mm; + dt = DT_SEGMENT; + } + } + + // Attempt to fill incomplete segment with deceleration ramp. + if ((dt < DT_SEGMENT) && (mm_remaining > 0.0)) { + if (mm_remaining <= prep.decelerate_after) { // Deceleration ramp + dt_remainder = DT_SEGMENT-dt; + partial_mm = dt_remainder*(prep.current_speed-0.5*pl_block->acceleration*dt_remainder); + if ((partial_mm > 0.0) && (mm_remaining > partial_mm)) { // **Complete** Segment filled. + prep.current_speed -= pl_block->acceleration*dt_remainder; + if (prep.current_speed <= prep.exit_speed) { + mm_remaining = 0.0; + + } else { + mm_remaining -= partial_mm; + dt = DT_SEGMENT; + } + } else { // **Complete** End of block. + dt += (2.0*mm_remaining/(prep.current_speed+prep.exit_speed)); + mm_remaining = 0.0; + // prep.current_speed = prep.exit_speed; + } + } + } + } + */ +// printString(" Z"); +// printFloat(dt*(60.0*1000.0)); +// printString(" "); +// printFloat(mm_remaining); +// printString(" "); +// printFloat(prep.current_speed); +// printString("Z "); + + /* ----------------------------------------------------------------------------------- + Compute segment step rate, steps to execute, and step phase correction parameters. + */ +// float step_events; +// if (mm_remaining > 0.0) { +// step_events = prep.step_per_mm*(pl_block->millimeters - mm_remaining); // Convert mm to steps +// prep_segment->n_step = floor(step_events + prep.step_remainder); +// if (prep_segment->n_step > prep.step_events_remaining) { // Prevent round-off overshoot +// prep_segment->n_step = prep.step_events_remaining; +// } +// } else { // Ensure all remaining steps are executed +// step_events = prep.step_per_mm*pl_block->millimeters; +// prep_segment->n_step = prep.step_events_remaining; +// } +// prep.step_events_remaining -= prep_segment->n_step; +// +// // Compute segment rate. +// prep_segment->dist_per_tick = +// ceil( (INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND)) * (step_events/dt) ); // (mult*step/isr_tic) +// +// if (prep.step_events_remaining > 0) { +// // Compute step phase distance and update segment continuation parameters. +// prep.step_remainder += step_events - prep_segment->n_step; +// prep_segment->phase_dist = ceil(INV_TIME_MULTIPLIER-INV_TIME_MULTIPLIER*prep.step_remainder); +// pl_block->millimeters = mm_remaining; +// pl_block->entry_speed_sqr = prep.current_speed*prep.current_speed; +// +// } else { // End of block. Finish it out. +// // The planner block is complete. All steps are set to be executed in the segment buffer. +// // Move planner pointer to next block and flag to load a new block for the next segment. +// prep_segment->phase_dist = INV_TIME_MULTIPLIER; +// pl_block = NULL; +// plan_discard_current_block(); +// } + + if (mm_remaining > 0.0) { + + float steps_remaining = prep.step_per_mm*mm_remaining; + prep_segment->dist_per_tick = ceil( (INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))* + ((prep.step_events_remaining-steps_remaining)/dt) ); // (mult*step/isr_tic) + + // Compute number of steps to execute and segment step phase correction. + prep_segment->n_step = ceil(prep.step_events_remaining)-ceil(steps_remaining); + prep_segment->phase_dist = ceil(INV_TIME_MULTIPLIER*(1.0-ceil(steps_remaining)+steps_remaining)); + + // Update step execution variables + prep.step_events_remaining = steps_remaining; + pl_block->millimeters = mm_remaining; + pl_block->entry_speed_sqr = prep.current_speed*prep.current_speed; + + } else { // End of block. Finish it out. + + prep_segment->dist_per_tick = ceil( (INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))* + prep.step_events_remaining/dt ); // (mult*step/isr_tic) + prep_segment->phase_dist = INV_TIME_MULTIPLIER; + + // Set to execute the remaining steps and no phase correction upon finishing the block. + prep_segment->n_step = ceil(prep.step_events_remaining); + + + // NOTE: Not required. Planner will ignore this block as it is now complete. + // prep.step_events_remaining = 0.0; + // pl_block->millimeters = 0.0; + + // The planner block is complete. All steps are set to be executed in the segment buffer. + // Move planner pointer to next block and flag to load a new block for the next segment. + pl_block = NULL; + plan_discard_current_block(); + } + +// long a = prep_segment->n_step; +// printInteger(a); +// printString(" "); +// a = prep_segment->phase_dist; +// printInteger(prep_segment->dist_per_tick); +// printString(" "); +// printFloat(prep.step_events_remaining); +// printString(" "); +// printFloat(pl_block->millimeters); +// printString(" "); + + + // !!! PROBLEM. Step events remaining in floating point can limit the number of steps + // we can accurately track, since floats have ~7.2 significant digits. However, this only + // becomes a problem if there are more than 1,000,000, which translates to a CNC machine + // with 200 step/mm and 5 meters of axis travel. Possible but unlikely. Could have more + // issues with user setting up their machine with too high of steps. + + // TODO: dist_per_tick must be less than INV_TIME_MULTIPLIER. A check can be made to + // make this a hard limit. Need to make sure this doesn't affect the velocity profiles.. + // it shouldn't. The same could said for the minimum allowable step rate too. This should + // not affect the tracing of the profiles either. + + // Ensure the initial step rate exceeds the MINIMUM_STEP_RATE. + // TODO: Use config.h error checking to do this. Otherwise, counters get screwy. + + // New step segment initialization completed. Increment segment buffer indices. + segment_buffer_head = segment_next_head; + if ( ++segment_next_head == SEGMENT_BUFFER_SIZE ) { segment_next_head = 0; } + +int32_t blength = segment_buffer_head - segment_buffer_tail; +if (blength < 0) { blength += SEGMENT_BUFFER_SIZE; } +printInteger(blength); +// SPINDLE_ENABLE_PORT ^= 1<entry_speed_sqr = exit_speed_sqr - 2*partial_block->acceleration*millimeters_remaining; - } else { // Block is accelerating or cruising - partial_block->entry_speed_sqr += 2*partial_block->acceleration*(partial_block->millimeters-millimeters_remaining); - partial_block->entry_speed_sqr = min(partial_block->entry_speed_sqr, partial_block->nominal_speed_sqr); - } - - // Update only the relevant planner block information so the planner can plan correctly. - partial_block->millimeters = millimeters_remaining; - partial_block->max_entry_speed_sqr = partial_block->entry_speed_sqr; // Not sure if this needs to be updated. - } -} /* PLANNER SPEED DEFINITION @@ -153,17 +115,11 @@ void plan_update_partial_block(uint8_t block_index, float exit_speed_sqr) the buffer is full or empty. As described for standard ring buffers, this block is always empty. - next_buffer_head: Points to next planner buffer block after the buffer head block. When equal to the buffer tail, this indicates the buffer is full. - - block_buffer_safe: Points to the first sequential planner block for which it is safe to recompute, which - is defined to be where the stepper's step segment buffer ends. This may or may not be the buffer tail, - since the step segment buffer queues steps which may have not finished executing and could span a few - blocks, if the block moves are very short. - block_buffer_planned: Points to the first buffer block after the last optimally planned block for normal streaming operating conditions. Use for planning optimizations by avoiding recomputing parts of the - planner buffer that don't change with the addition of a new block, as describe above. - - NOTE: All planner computations are performed in floating point to minimize numerical round-off errors. - When a planner block is executed, the floating point values are converted to fast integers by the stepper - algorithm segment buffer. See the stepper module for details. + planner buffer that don't change with the addition of a new block, as describe above. In addition, + this block can never be less than block_buffer_tail and will always be pushed forward and maintain + this requirement when encountered by the plan_discard_current_block() routine during a cycle. NOTE: Since the planner only computes on what's in the planner buffer, some motions with lots of short line segments, like G2/3 arcs or complex curves, may seem to move slow. This is because there simply isn't @@ -180,175 +136,110 @@ void plan_update_partial_block(uint8_t block_index, float exit_speed_sqr) */ static void planner_recalculate() { - // Initialize block index to the last block in the planner buffer. uint8_t block_index = plan_prev_block_index(block_buffer_head); - - // Query stepper module for safe planner block index to recalculate to, which corresponds to the end - // of the step segment buffer. - uint8_t block_buffer_safe = st_get_prep_block_index(); - - // TODO: Make sure that we don't have to check for the block_buffer_tail condition, if the stepper module - // returns a NULL pointer or something. This could happen when the segment buffer is empty. Although, - // this call won't return a NULL, only an index.. I have to make sure that this index is synced with the - // planner at all times. - - // Recompute plan only when there is more than one planner block in the buffer. Can't do anything with one. - // NOTE: block_buffer_safe can be the last planner block if the segment buffer has completely queued up the - // remainder of the planner buffer. In this case, a new planner block will be treated as a single block. - if (block_index == block_buffer_safe) { // Also catches (head-1) = tail - - // Just set block_buffer_planned pointer. - block_buffer_planned = block_index; - - // TODO: Feedrate override of one block needs to update the partial block with an exit speed of zero. For - // a single added block and recalculate after a feed hold, we don't need to compute this, since we already - // know that the velocity starts and ends at zero. With an override, we can be traveling at some midblock - // rate, and we have to calculate the new velocity profile from it. - // plan_update_partial_block(block_index,0.0); - - } else { - - // TODO: If the nominal speeds change during a feedrate override, we need to recompute the max entry speeds for - // all junctions before proceeding. - - // Initialize planner buffer pointers and indexing. - plan_block_t *current = &block_buffer[block_index]; - - // Calculate maximum entry speed for last block in buffer, where the exit speed is always zero. - current->entry_speed_sqr = min( current->max_entry_speed_sqr, 2*current->acceleration*current->millimeters); + + // Bail. Can't do anything with one only one plan-able block. + if (block_index == block_buffer_planned) { return; } - // Reverse Pass: Coarsely maximize all possible deceleration curves back-planning from the last - // block in buffer. Cease planning when: (1) the last optimal planned pointer is reached. - // (2) the safe block pointer is reached, whereby the planned pointer is updated. - // NOTE: Forward pass will later refine and correct the reverse pass to create an optimal plan. - // NOTE: If the safe block is encountered before the planned block pointer, we know the safe block - // will be recomputed within the plan. So, we need to update it if it is partially completed. - float entry_speed_sqr; - plan_block_t *next; - block_index = plan_prev_block_index(block_index); + // Reverse Pass: Coarsely maximize all possible deceleration curves back-planning from the last + // block in buffer. Cease planning when the last optimal planned or tail pointer is reached. + // NOTE: Forward pass will later refine and correct the reverse pass to create an optimal plan. + float entry_speed_sqr; + plan_block_t *next; + plan_block_t *current = &block_buffer[block_index]; - if (block_index == block_buffer_safe) { // !! OR plan pointer? Yes I think so. - - // Only two plannable blocks in buffer. Compute previous block based on - // !!! May only work if a new block is being added. Not for an override. The exit speed isn't zero. - // !!! Need to make the current entry speed calculation after this. - plan_update_partial_block(block_index, 0.0); - block_buffer_planned = block_index; - - } else { - - // Three or more plan-able - while (block_index != block_buffer_planned) { + // Calculate maximum entry speed for last block in buffer, where the exit speed is always zero. + current->entry_speed_sqr = min( current->max_entry_speed_sqr, 2*current->acceleration*current->millimeters); + + block_index = plan_prev_block_index(block_index); + if (block_index == block_buffer_planned) { // Only two plannable blocks in buffer. Reverse pass complete. + // Check if the first block is the tail. If so, notify stepper to update its current parameters. + if (block_index == block_buffer_tail) { st_update_plan_block_parameters(); } + } else { // Three or more plan-able blocks + while (block_index != block_buffer_planned) { + next = current; + current = &block_buffer[block_index]; + block_index = plan_prev_block_index(block_index); - next = current; - current = &block_buffer[block_index]; + // Check if next block is the tail block(=planned block). If so, update current stepper parameters. + if (block_index == block_buffer_tail) { st_update_plan_block_parameters(); } - // Increment block index early to check if the safe block is before the current block. If encountered, - // this is an exit condition as we can't go further than this block in the reverse pass. - block_index = plan_prev_block_index(block_index); - if (block_index == block_buffer_safe) { - // Check if the safe block is partially completed. If so, update it before its exit speed - // (=current->entry speed) is over-written. - // TODO: The update breaks with feedrate overrides, because the replanning process no longer has - // the previous nominal speed to update this block with. There will need to be something along the - // lines of a nominal speed change check and send the correct value to this function. - plan_update_partial_block(block_index,current->entry_speed_sqr); - - // Set planned pointer at safe block and for loop exit after following computation is done. - block_buffer_planned = block_index; - } - - // Compute maximum entry speed decelerating over the current block from its exit speed. - if (current->entry_speed_sqr != current->max_entry_speed_sqr) { - entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters; - if (entry_speed_sqr < current->max_entry_speed_sqr) { - current->entry_speed_sqr = entry_speed_sqr; - } else { - current->entry_speed_sqr = current->max_entry_speed_sqr; - } + // Compute maximum entry speed decelerating over the current block from its exit speed. + if (current->entry_speed_sqr != current->max_entry_speed_sqr) { + entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters; + if (entry_speed_sqr < current->max_entry_speed_sqr) { + current->entry_speed_sqr = entry_speed_sqr; + } else { + current->entry_speed_sqr = current->max_entry_speed_sqr; } } - - } + } + } - // Forward Pass: Forward plan the acceleration curve from the planned pointer onward. - // Also scans for optimal plan breakpoints and appropriately updates the planned pointer. - next = &block_buffer[block_buffer_planned]; // Begin at buffer planned pointer - block_index = plan_next_block_index(block_buffer_planned); - while (block_index != block_buffer_head) { - current = next; - next = &block_buffer[block_index]; - - // Any acceleration detected in the forward pass automatically moves the optimal planned - // pointer forward, since everything before this is all optimal. In other words, nothing - // can improve the plan from the buffer tail to the planned pointer by logic. - // TODO: Need to check if the planned flag logic is correct for all scenarios. It may not - // be for certain conditions. However, if the block reaches nominal speed, it can be a valid - // breakpoint substitute. - if (current->entry_speed_sqr < next->entry_speed_sqr) { - entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters; - // If true, current block is full-acceleration and we can move the planned pointer forward. - if (entry_speed_sqr < next->entry_speed_sqr) { - next->entry_speed_sqr = entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass sets this. - block_buffer_planned = block_index; // Set optimal plan pointer. - } + // Forward Pass: Forward plan the acceleration curve from the planned pointer onward. + // Also scans for optimal plan breakpoints and appropriately updates the planned pointer. + next = &block_buffer[block_buffer_planned]; // Begin at buffer planned pointer + block_index = plan_next_block_index(block_buffer_planned); + while (block_index != block_buffer_head) { + current = next; + next = &block_buffer[block_index]; + + // Any acceleration detected in the forward pass automatically moves the optimal planned + // pointer forward, since everything before this is all optimal. In other words, nothing + // can improve the plan from the buffer tail to the planned pointer by logic. + if (current->entry_speed_sqr < next->entry_speed_sqr) { + entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters; + // If true, current block is full-acceleration and we can move the planned pointer forward. + if (entry_speed_sqr < next->entry_speed_sqr) { + next->entry_speed_sqr = entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass sets this. + block_buffer_planned = block_index; // Set optimal plan pointer. } - - // Any block set at its maximum entry speed also creates an optimal plan up to this - // point in the buffer. When the plan is bracketed by either the beginning of the - // buffer and a maximum entry speed or two maximum entry speeds, every block in between - // cannot logically be further improved. Hence, we don't have to recompute them anymore. - if (next->entry_speed_sqr == next->max_entry_speed_sqr) { - block_buffer_planned = block_index; // Set optimal plan pointer - } - - block_index = plan_next_block_index( block_index ); } - } - -} - - -void plan_reset_buffer() -{ - block_buffer_planned = block_buffer_tail; + // Any block set at its maximum entry speed also creates an optimal plan up to this + // point in the buffer. When the plan is bracketed by either the beginning of the + // buffer and a maximum entry speed or two maximum entry speeds, every block in between + // cannot logically be further improved. Hence, we don't have to recompute them anymore. + if (next->entry_speed_sqr == next->max_entry_speed_sqr) { block_buffer_planned = block_index; } + block_index = plan_next_block_index( block_index ); + } } void plan_init() { + memset(&pl, 0, sizeof(pl)); // Clear planner struct block_buffer_tail = 0; block_buffer_head = 0; // Empty = tail next_buffer_head = 1; // plan_next_block_index(block_buffer_head) - plan_reset_buffer(); - memset(&pl, 0, sizeof(pl)); // Clear planner struct + block_buffer_planned = 0; // = block_buffer_tail; } void plan_discard_current_block() { if (block_buffer_head != block_buffer_tail) { // Discard non-empty buffer. - block_buffer_tail = plan_next_block_index( block_buffer_tail ); + uint8_t block_index = plan_next_block_index( block_buffer_tail ); + // Push block_buffer_planned pointer, if encountered. + if (block_buffer_tail == block_buffer_planned) { block_buffer_planned = block_index; } + block_buffer_tail = block_index; } } plan_block_t *plan_get_current_block() { - if (block_buffer_head == block_buffer_tail) { // Buffer empty - plan_reset_buffer(); - return(NULL); - } + if (block_buffer_head == block_buffer_tail) { return(NULL); } // Buffer empty return(&block_buffer[block_buffer_tail]); } -plan_block_t *plan_get_block_by_index(uint8_t block_index) +float plan_get_exec_block_exit_speed() { - if (block_buffer_head == block_index) { return(NULL); } - return(&block_buffer[block_index]); + uint8_t block_index = plan_next_block_index(block_buffer_tail); + if (block_index == block_buffer_head) { return( 0.0 ); } + return( sqrt( block_buffer[block_index].entry_speed_sqr ) ); } @@ -508,12 +399,10 @@ void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) // Finish up by recalculating the plan with the new block. planner_recalculate(); - + // int32_t blength = block_buffer_head - block_buffer_tail; // if (blength < 0) { blength += BLOCK_BUFFER_SIZE; } // printInteger(blength); - - } @@ -527,68 +416,6 @@ void plan_sync_position() } -/* STEPPER VELOCITY PROFILE DEFINITION - less than nominal speed-> + - +--------+ <- nominal_speed /|\ - / \ / | \ - entry_speed -> + \ / | + <- next->entry_speed - | + <- next->entry_speed / | | - +-------------+ entry_speed -> +----+--+ - time --> ^ ^ ^ ^ - | | | | - decelerate distance decelerate distance - - Calculates the type of velocity profile for a given planner block and provides the deceleration - distance for the stepper algorithm to use to accurately trace the profile exactly. The planner - computes the entry and exit speeds of each block, but does not bother to determine the details of - the velocity profiles within them, as they aren't needed for computing an optimal plan. When the - stepper algorithm begins to execute a block, the block velocity profiles are computed ad hoc. - - Each block velocity profiles can be described as either a trapezoidal or a triangular shape. The - trapezoid occurs when the block reaches the nominal speed of the block and cruises for a period of - time. A triangle occurs when the nominal speed is not reached within the block. Both of these - velocity profiles may also be truncated on either end with no acceleration or deceleration ramps, - as they can be influenced by the conditions of neighboring blocks, where the acceleration ramps - are defined by constant acceleration equal to the maximum allowable acceleration of a block. - - Since the stepper algorithm already assumes to begin executing a planner block by accelerating - from the planner entry speed and cruise if the nominal speed is reached, we only need to know - when to begin deceleration to the end of the block. Hence, only the distance from the end of the - block to begin a deceleration ramp is computed for the stepper algorithm when requested. -*/ -float plan_calculate_velocity_profile(uint8_t block_index) -{ - plan_block_t *current_block = &block_buffer[block_index]; - - // Determine current block exit speed - float exit_speed_sqr = 0.0; // Initialize for end of planner buffer. Zero speed. - plan_block_t *next_block = plan_get_block_by_index(plan_next_block_index(block_index)); - if (next_block != NULL) { exit_speed_sqr = next_block->entry_speed_sqr; } // Exit speed is the entry speed of next buffer block - - // First determine intersection distance (in steps) from the exit point for a triangular profile. - // Computes: d_intersect = distance/2 + (v_entry^2-v_exit^2)/(4*acceleration) - float intersect_distance = 0.5*( current_block->millimeters + (current_block->entry_speed_sqr-exit_speed_sqr)/(2*current_block->acceleration) ); - - // Check if this is a pure acceleration block by a intersection distance less than zero. Also - // prevents signed and unsigned integer conversion errors. - if (intersect_distance > 0 ) { - float decelerate_distance; - // Determine deceleration distance (in steps) from nominal speed to exit speed for a trapezoidal profile. - // Value is never negative. Nominal speed is always greater than or equal to the exit speed. - // Computes: d_decelerate = (v_nominal^2 - v_exit^2)/(2*acceleration) - decelerate_distance = (current_block->nominal_speed_sqr - exit_speed_sqr)/(2*current_block->acceleration); - - // The lesser of the two triangle and trapezoid distances always defines the velocity profile. - if (decelerate_distance > intersect_distance) { decelerate_distance = intersect_distance; } - - // Finally, check if this is a pure deceleration block. - if (decelerate_distance > current_block->millimeters) { return(0.0); } - else { return( (current_block->millimeters-decelerate_distance) ); } - } - return( current_block->millimeters ); // No deceleration in velocity profile. -} - - // Re-initialize buffer plan with a partially completed block, assumed to exist at the buffer tail. // Called after a steppers have come to a complete stop for a feed hold and the cycle is stopped. void plan_cycle_reinitialize(int32_t step_events_remaining) @@ -607,63 +434,3 @@ void plan_cycle_reinitialize(int32_t step_events_remaining) block_buffer_planned = block_buffer_tail; planner_recalculate(); } - - -/* -TODO: - When a feed hold or feedrate override is reduced, the velocity profile must execute a - deceleration over the existing plan. By logic, since the plan already decelerates to zero - at the end of the buffer, any replanned deceleration mid-way will never exceed this. It - will only asymptotically approach this in the worst case scenario. - - - For a feed hold, we simply need to plan and compute the stopping point within a block - when velocity decelerates to zero. We then can recompute the plan with the already - existing partial block planning code and set the system to a QUEUED state. - - When a feed hold is initiated, the main program should be able to continue doing what - it has been, i.e. arcs, parsing, but needs to be able to reinitialize the plan after - it has come to a stop. - - - For a feed rate override (reduce-only), we need to enforce a deceleration until we - intersect the reduced nominal speed of a block after it's been planned with the new - overrides and the newly planned block is accelerating or cruising only. If the new plan - block is decelerating at the intersection point, we keep decelerating until we find a - valid intersection point. Once we find this point, we can then resume onto the new plan, - but we may need to adjust the deceleration point in the intersection block since the - feedrate override could have intersected at an acceleration ramp. This would change the - acceleration ramp to a cruising, so the deceleration point will have changed, but the - plan will have not. It should still be valid for the rest of the buffer. Coding this - can get complicated, but it should be doable. One issue could be is in how to handle - scenarios when a user issues several feedrate overrides and inundates this code. Does - this method still work and is robust enough to compute all of this on the fly? This is - the critical question. However, we could block user input until the planner has time to - catch to solve this as well. - - - When the feed rate override increases, we don't have to do anything special. We just - replan the entire buffer with the new nominal speeds and adjust the maximum junction - speeds accordingly. - -void plan_compute_deceleration() { - -} - - -void plan_recompute_max_junction_velocity() { - // Assumes the nominal_speed_sqr values have been updated. May need to just multiply - // override values here. - // PROBLEM: Axes-limiting velocities get screwed up. May need to store an int8 value for the - // max override value possible for each block when the line is added. So the nominal_speed - // is computed with that ceiling, but still retained if the rates change again. - uint8_t block_index = block_buffer_tail; - plan_block_t *block = &block_buffer[block_index]; - pl.previous_nominal_speed_sqr = block->nominal_speed_sqr; - block_index = plan_next_block_index(block_index); - while (block_index != block_buffer_head) { - block = &block_buffer[block_index]; - block->max_entry_speed_sqr = min(block->max_junction_speed_sqr, - min(block->nominal_speed_sqr,pl.previous_nominal_speed_sqr)); - pl.previous_nominal_speed_sqr = block->nominal_speed_sqr; - block_index = plan_next_block_index(block_index); - } -} - -*/ diff --git a/planner.h b/planner.h index a52a4f5..23f254a 100644 --- a/planner.h +++ b/planner.h @@ -35,8 +35,8 @@ typedef struct { // Fields used by the bresenham algorithm for tracing the line // NOTE: Do not change any of these values once set. The stepper algorithm uses them to execute the block correctly. uint8_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) - int32_t steps[N_AXIS]; // Step count along each axis - int32_t step_event_count; // The maximum step axis count and number of steps required to complete this block. + int32_t steps[N_AXIS]; // Step count along each axis + int32_t step_event_count; // The maximum step axis count and number of steps required to complete this block. // Fields used by the motion planner to manage acceleration float entry_speed_sqr; // The current planned entry speed at block junction in (mm/min)^2 @@ -44,10 +44,11 @@ typedef struct { // neighboring nominal speeds with overrides in (mm/min)^2 float max_junction_speed_sqr; // Junction entry speed limit based on direction vectors in (mm/min)^2 float nominal_speed_sqr; // Axis-limit adjusted nominal speed for this block in (mm/min)^2 - float acceleration; // Axis-limit adjusted line acceleration in mm/min^2 - float millimeters; // The remaining distance for this block to be executed in mm + float acceleration; // Axis-limit adjusted line acceleration in (mm/min^2) + float millimeters; // The remaining distance for this block to be executed in (mm) } plan_block_t; + // Initialize the motion plan subsystem void plan_init(); @@ -64,13 +65,11 @@ void plan_discard_current_block(); // Gets the current block. Returns NULL if buffer empty plan_block_t *plan_get_current_block(); +// Called periodically by step segment buffer. Mostly used internally by planner. uint8_t plan_next_block_index(uint8_t block_index); -plan_block_t *plan_get_block_by_index(uint8_t block_index); - -float plan_calculate_velocity_profile(uint8_t block_index); - -// void plan_update_partial_block(uint8_t block_index, float millimeters_remaining, uint8_t is_decelerating); +// Called by step segment buffer when computing executing block velocity profile. +float plan_get_exec_block_exit_speed(); // Reset the planner position vector (in steps) void plan_sync_position(); diff --git a/stepper.c b/stepper.c index e3e4b25..ea78b2e 100644 --- a/stepper.c +++ b/stepper.c @@ -26,25 +26,41 @@ #include "planner.h" #include "nuts_bolts.h" + // Some useful constants #define TICKS_PER_MICROSECOND (F_CPU/1000000) +#define DT_SEGMENT (1.0/(ACCELERATION_TICKS_PER_SECOND*60.0)) // min/segment -#define RAMP_NOOP_CRUISE 0 -#define RAMP_ACCEL 1 +#define RAMP_ACCEL 0 +#define RAMP_CRUISE 1 #define RAMP_DECEL 2 -#define LOAD_NOOP 0 -#define LOAD_SEGMENT 1 -#define LOAD_BLOCK 2 -#define SEGMENT_NOOP 0 -#define SEGMENT_END_OF_BLOCK bit(0) -#define RAMP_CHANGE_ACCEL bit(1) -#define RAMP_CHANGE_DECEL bit(2) +// Stores the planner block Bresenham algorithm execution data for the segments in the segment +// buffer. Normally, this buffer is partially in-use, but, for the worst case scenario, it will +// never exceed the number of accessible stepper buffer segments (SEGMENT_BUFFER_SIZE-1). +// NOTE: This data is copied from the prepped planner blocks so that the planner blocks may be +// discarded when entirely consumed and completed by the segment buffer. +typedef struct { + uint8_t direction_bits; + int32_t steps[N_AXIS]; + int32_t step_event_count; +} st_block_t; +static st_block_t st_block_buffer[SEGMENT_BUFFER_SIZE-1]; +// TODO: Directly adjust this parameters to stop motion of individual axes for the homing cycle. +// But this may require this to be volatile if it is controlled by an interrupt. -#define MINIMUM_STEPS_PER_SEGMENT 1 // Don't change - -#define SEGMENT_BUFFER_SIZE 6 +// Primary stepper segment ring buffer. Contains small, short line segments for the stepper +// algorithm to execute, which are "checked-out" incrementally from the first block in the +// planner buffer. Once "checked-out", the steps in the segments buffer cannot be modified by +// the planner, where the remaining planner block steps still can. +typedef struct { + uint8_t n_step; // Number of step events to be executed for this segment + uint8_t st_block_index; // Stepper block data index. Uses this information to execute this segment. + int32_t phase_dist; // Remaining step fraction to tick before completing segment. + int32_t dist_per_tick; // Step distance traveled per ISR tick, aka step rate. +} segment_t; +static segment_t segment_buffer[SEGMENT_BUFFER_SIZE]; // Stepper state variable. Contains running data and trapezoid variables. typedef struct { @@ -52,84 +68,90 @@ typedef struct { int32_t counter_x, // Counter variables for the bresenham line tracer counter_y, counter_z; - uint8_t segment_steps_remaining; // Steps remaining in line segment motion // Used by inverse time algorithm to track step rate - int32_t counter_dist; // Inverse time distance traveled since last step event - uint32_t ramp_rate; // Inverse time distance traveled per interrupt tick - uint32_t dist_per_tick; + int32_t counter_dist; // Inverse time distance traveled since last step event // Used by the stepper driver interrupt - uint8_t execute_step; // Flags step execution for each interrupt. - uint8_t step_pulse_time; // Step pulse reset time after step rise - uint8_t out_bits; // The next stepping-bits to be output - uint8_t load_flag; - - uint8_t counter_ramp; - uint8_t ramp_type; + uint8_t execute_step; // Flags step execution for each interrupt. + uint8_t step_pulse_time; // Step pulse reset time after step rise + uint8_t out_bits; // The next stepping-bits to be output + + uint8_t step_count; // Steps remaining in line segment motion + uint8_t exec_block_index; // Tracks the current st_block index. Change indicates new block. + st_block_t *exec_block; // Pointer to the block data for the segment being executed + segment_t *exec_segment; // Pointer to the segment being executed } stepper_t; static stepper_t st; -// Stores stepper common data for executing steps in the segment buffer. Data can change mid-block when the -// planner updates the remaining block velocity profile with a more optimal plan or a feedrate override occurs. -// NOTE: Normally, this buffer is partially in-use, but, for the worst case scenario, it will never exceed -// the number of accessible stepper buffer segments (SEGMENT_BUFFER_SIZE-1). -typedef struct { - int32_t step_events_remaining; // Tracks step event count for the executing planner block - uint32_t dist_per_step; // Scaled distance to next step - uint32_t initial_rate; // Initialized step rate at re/start of a planner block - uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute - uint32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive) - uint32_t current_approx_rate; // Tracks the approximate segment rate to predict steps per segment to execute - int32_t decelerate_after; // Tracks when to initiate deceleration according to the planner block - float mm_per_step; -} st_data_t; -static st_data_t segment_data[SEGMENT_BUFFER_SIZE-1]; - -// Primary stepper segment ring buffer. Contains small, short line segments for the stepper algorithm to execute, -// which are "checked-out" incrementally from the first block in the planner buffer. Once "checked-out", the steps -// in the segments buffer cannot be modified by the planner, where the remaining planner block steps still can. -typedef struct { - uint8_t n_step; // Number of step events to be executed for this segment - uint8_t st_data_index; // Stepper buffer common data index. Uses this information to execute this segment. - uint8_t flag; // Stepper algorithm bit-flag for special execution conditions. -} st_segment_t; -static st_segment_t segment_buffer[SEGMENT_BUFFER_SIZE]; - // Step segment ring buffer indices static volatile uint8_t segment_buffer_tail; -static volatile uint8_t segment_buffer_head; +static uint8_t segment_buffer_head; static uint8_t segment_next_head; -static volatile uint8_t busy; // Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though. -static plan_block_t *pl_current_block; // A pointer to the planner block currently being traced -static st_segment_t *st_current_segment; -static st_data_t *st_current_data; +// Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though. +static volatile uint8_t busy; // Pointers for the step segment being prepped from the planner buffer. Accessed only by the // main program. Pointers may be planning segments or planner blocks ahead of what being executed. -static plan_block_t *pl_prep_block; // Pointer to the planner block being prepped -static st_data_t *st_prep_data; // Pointer to the stepper common data being prepped -static uint8_t pl_prep_index; // Index of planner block being prepped -static uint8_t st_data_prep_index; // Index of stepper common data block being prepped -static uint8_t pl_partial_block_flag; // Flag indicating the planner has modified the prepped planner block +static plan_block_t *pl_block; // Pointer to the planner block being prepped +static st_block_t *st_prep_block; // Pointer to the stepper block data being prepped + +// Segment preparation data struct. Contains all the necessary information to compute new segments +// based on the current executing planner block. +typedef struct { + uint8_t st_block_index; // Index of stepper common data block being prepped + uint8_t partial_block_flag; // Flag indicating the planner has modified the prepped planner block + + float step_per_mm; // Current planner block step/millimeter conversion scalar + float step_events_remaining; // Tracks step event count for the executing planner block + + uint8_t ramp_type; // Current segment ramp state + float current_speed; // Current speed at the end of the segment buffer (mm/min) + float maximum_speed; // Maximum speed of executing block. Not always nominal speed. (mm/min) + float exit_speed; // Exit speed of executing block (mm/min) + float accelerate_until; // Acceleration ramp end measured from end of block (mm) + float decelerate_after; // Deceleration ramp start measured from end of block (mm) +} st_prep_t; +static st_prep_t prep; -/* __________________________ +/* BLOCK VELOCITY PROFILE DEFINITION + __________________________ /| |\ _________________ ^ / | | \ /| |\ | / | | \ / | | \ s / | | | | | \ p / | | | | | \ e +-----+------------------------+---+--+---------------+----+ e - | BLOCK 1 | BLOCK 2 | d - - time -----> + | BLOCK 1 ^ BLOCK 2 | d + | + time -----> EXAMPLE: Block 2 entry speed is at max junction velocity - The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta - until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after - after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as - +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. + The planner block buffer is planned assuming constant acceleration velocity profiles and are + continuously joined at block junctions as shown above. However, the planner only actively computes + the block entry speeds for an optimal velocity plan, but does not compute the block internal + velocity profiles. These velocity profiles are computed ad-hoc as they are executed by the + stepper algorithm and consists of only 7 possible types of profiles: cruise-only, cruise- + deceleration, acceleration-cruise, acceleration-only, deceleration-only, full-trapezoid, and + triangle(no cruise). + + maximum_speed (< nominal_speed) -> + + +--------+ <- maximum_speed (= nominal_speed) /|\ + / \ / | \ + current_speed -> + \ / | + <- exit_speed + | + <- exit_speed / | | + +-------------+ current_speed -> +----+--+ + time --> ^ ^ ^ ^ + | | | | + decelerate_after(in mm) decelerate_after(in mm) + ^ ^ ^ ^ + | | | | + accelerate_until(in mm) accelerate_until(in mm) + + The step segment buffer computes the executing block velocity profile and tracks the critical + parameters for the stepper algorithm to accurately trace the profile. These critical parameters + are shown and defined in the above illustration. */ // Stepper state initialization. Cycle should only start if the st.cycle_start flag is @@ -147,10 +169,8 @@ void st_wake_up() st.out_bits = settings.invert_mask; // Initialize step pulse timing from settings. st.step_pulse_time = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3); - // Enable stepper driver interrupt - st.execute_step = false; - st.load_flag = LOAD_BLOCK; + // Enable stepper driver interrupt TCNT2 = 0; // Clear Timer2 TIMSK2 |= (1<n_step; + st.exec_segment = &segment_buffer[segment_buffer_tail]; + st.step_count = st.exec_segment->n_step; // If the new segment starts a new planner block, initialize stepper variables and counters. - // NOTE: For new segments only, the step counters are not updated to ensure step phasing is continuous. - if (st.load_flag == LOAD_BLOCK) { - pl_current_block = plan_get_current_block(); // Should always be there. Stepper buffer handles this. - st_current_data = &segment_data[segment_buffer[segment_buffer_tail].st_data_index]; + // NOTE: When the segment data index changes, this indicates a new planner block. + if ( st.exec_block_index != st.exec_segment->st_block_index ) { + st.exec_block_index = st.exec_segment->st_block_index; + st.exec_block = &st_block_buffer[st.exec_block_index]; // Initialize direction bits for block. Set execute flag to set directions bits upon next ISR tick. - st.out_bits = pl_current_block->direction_bits ^ settings.invert_mask; + st.out_bits = st.exec_block->direction_bits ^ settings.invert_mask; st.execute_step = true; - // Initialize Bresenham line counters - st.counter_x = (pl_current_block->step_event_count >> 1); + // Initialize Bresenham line and distance counters + st.counter_x = (st.exec_block->step_event_count >> 1); st.counter_y = st.counter_x; st.counter_z = st.counter_x; - - // Initialize inverse time, step rate data, and acceleration ramp counters - st.counter_dist = st_current_data->dist_per_step; // dist_per_step always greater than ramp_rate. - st.ramp_rate = st_current_data->initial_rate; - st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK/2; // Initialize ramp counter via midpoint rule - st.ramp_type = RAMP_NOOP_CRUISE; // Initialize as no ramp operation. Corrected later if necessary. - - // Ensure the initial step rate exceeds the MINIMUM_STEP_RATE. - if (st.ramp_rate < MINIMUM_STEP_RATE) { st.dist_per_tick = MINIMUM_STEP_RATE; } - else { st.dist_per_tick = st.ramp_rate; } - } - - // Check if ramp conditions have changed. If so, update ramp counters and control variables. - if ( st_current_segment->flag & (RAMP_CHANGE_DECEL | RAMP_CHANGE_ACCEL) ) { - /* Compute correct ramp count for a ramp change. Upon a switch from acceleration to deceleration, - or vice-versa, the new ramp count must be set to trigger the next acceleration tick equal to - the number of ramp ISR ticks counted since the last acceleration tick. This is ensures the - ramp is executed exactly as the plan dictates. Otherwise, when a ramp begins from a known - rate (nominal/cruise or initial), the ramp count must be set to ISR_TICKS_PER_ACCELERATION_TICK/2 - as mandated by the mid-point rule. For the latter conditions, the ramp count have been - initialized such that the following computation is still correct. */ - st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK-st.counter_ramp; - if ( st_current_segment->flag & RAMP_CHANGE_DECEL ) { st.ramp_type = RAMP_DECEL; } - else { st.ramp_type = RAMP_ACCEL; } + st.counter_dist = 0; } - st.load_flag = LOAD_NOOP; // Segment motion loaded. Set no-operation flag to skip during execution. - } else { - // Can't discard planner block here if a feed hold stops in middle of block. + // Segment buffer empty. Shutdown. st_go_idle(); bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end return; // Nothing to do but exit. } } - - // Adjust inverse time counter for ac/de-celerations - if (st.ramp_type) { // Ignored when ramp type is RAMP_NOOP_CRUISE - st.counter_ramp--; // Tick acceleration ramp counter - if (st.counter_ramp == 0) { // Adjust step rate when its time - st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter - if (st.ramp_type == RAMP_ACCEL) { // Adjust velocity for acceleration - st.ramp_rate += st_current_data->rate_delta; - if (st.ramp_rate >= st_current_data->nominal_rate) { // Reached nominal rate. - st.ramp_rate = st_current_data->nominal_rate; // Set cruising velocity - st.ramp_type = RAMP_NOOP_CRUISE; // Set ramp flag to cruising - st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK/2; // Re-initialize counter for next ramp change. - } - } else { // Adjust velocity for deceleration. - if (st.ramp_rate > st_current_data->rate_delta) { - st.ramp_rate -= st_current_data->rate_delta; - } else { // Moving near zero feed rate. Gracefully slow down. - st.ramp_rate >>= 1; // Integer divide by 2 until complete. Also prevents overflow. - } - } - // Adjust for minimum step rate, but retain operating ramp rate for accurate velocity tracing. - if (st.ramp_rate < MINIMUM_STEP_RATE) { st.dist_per_tick = MINIMUM_STEP_RATE; } - else { st.dist_per_tick = st.ramp_rate; } - } - } - + // Iterate inverse time counter. Triggers each Bresenham step event. - st.counter_dist -= st.dist_per_tick; + st.counter_dist += st.exec_segment->dist_per_tick; // Execute Bresenham step event, when it's time to do so. - if (st.counter_dist < 0) { - st.counter_dist += st_current_data->dist_per_step; // Reload inverse time counter + if (st.counter_dist > INV_TIME_MULTIPLIER) { + if (st.step_count > 0) { // Block phase correction from executing step. + st.counter_dist -= INV_TIME_MULTIPLIER; // Reload inverse time counter + st.step_count--; // Decrement step events count - st.out_bits = pl_current_block->direction_bits; // Reset out_bits and reload direction bits - st.execute_step = true; - - // Execute step displacement profile by Bresenham line algorithm - st.counter_x -= pl_current_block->steps[X_AXIS]; - if (st.counter_x < 0) { - st.out_bits |= (1<step_event_count; - if (st.out_bits & (1<steps[Y_AXIS]; - if (st.counter_y < 0) { - st.out_bits |= (1<step_event_count; - if (st.out_bits & (1<steps[Z_AXIS]; - if (st.counter_z < 0) { - st.out_bits |= (1<step_event_count; - if (st.out_bits & (1<flag & SEGMENT_END_OF_BLOCK) { - plan_discard_current_block(); - st.load_flag = LOAD_BLOCK; - } else { - st.load_flag = LOAD_SEGMENT; + // Execute step displacement profile by Bresenham line algorithm + st.execute_step = true; + st.out_bits = st.exec_block->direction_bits; // Reset out_bits and reload direction bits + st.counter_x -= st.exec_block->steps[X_AXIS]; + if (st.counter_x < 0) { + st.out_bits |= (1<step_event_count; + if (st.out_bits & (1<steps[Y_AXIS]; + if (st.counter_y < 0) { + st.out_bits |= (1<step_event_count; + if (st.out_bits & (1<steps[Z_AXIS]; + if (st.counter_z < 0) { + st.out_bits |= (1<step_event_count; + if (st.out_bits & (1< st.exec_segment->phase_dist) { + // Segment is complete. Discard current segment and advance segment indexing. + st.exec_segment = NULL; if ( ++segment_buffer_tail == SEGMENT_BUFFER_SIZE) { segment_buffer_tail = 0; } } - - st.out_bits ^= settings.invert_mask; // Apply step port invert mask } + busy = false; // SPINDLE_ENABLE_PORT ^= 1<step_events_remaining); +// if (pl_current_block != NULL) { +// plan_cycle_reinitialize(st_exec_block->step_events_remaining); // st.ramp_type = RAMP_ACCEL; // st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK/2; // st.ramp_rate = 0; @@ -480,6 +425,17 @@ void st_cycle_reinitialize() } +// Called by planner_recalculate() when the executing block is updated by the new plan. +void st_update_plan_block_parameters() +{ + if (pl_block != NULL) { // Ignore if at start of a new block. + pl_block->entry_speed_sqr = prep.current_speed*prep.current_speed; // Update entry speed. + pl_block = NULL; // Flag st_prep_segment() to load new velocity profile. + prep.partial_block_flag = true; // Flag st_prep_segment() to indicate block continuation. + } +} + + /* Prepares step segment buffer. Continuously called from main program. The segment buffer is an intermediary buffer interface between the execution of steps @@ -490,214 +446,247 @@ void st_cycle_reinitialize() The number of steps "checked-out" from the planner buffer and the number of segments in the segment buffer is sized and computed such that no operation in the main program takes longer than the time it takes the stepper algorithm to empty it before refilling it. - Currently, the segment buffer conservatively holds roughly up to 40-60 msec of steps. + Currently, the segment buffer conservatively holds roughly up to 40-50 msec of steps. - NOTE: The segment buffer executes a set number of steps over an approximate time period. - If we try to execute over a fixed time period, it is difficult to guarantee or predict - how many steps will execute over it, especially when the step pulse phasing between the - neighboring segments must also be kept consistent. Meaning that, if the last segment step - pulses right before a segment end, the next segment must delay its first pulse so that the - step pulses are consistently spaced apart over time to keep the step pulse train nice and - smooth. Keeping track of phasing and ensuring that the exact number of steps are executed - as defined by the planner block, the related computational overhead can get quickly and - prohibitively expensive, especially in real-time. - Since the stepper algorithm automatically takes care of the step pulse phasing with - its ramp and inverse time counters by retaining the count remainders, we don't have to - explicitly and expensively track and synchronize the exact number of steps, time, and - phasing of steps. All we need to do is approximate the number of steps in each segment - such that the segment buffer has enough execution time for the main program to do what - it needs to do and refill it when it comes back. In other words, we just need to compute - a cheap approximation of the current velocity and the number of steps over it. + NOTE: The segment buffer executes a computed number of steps over a configured segment + execution time period, except at an end of a planner block where the segment execution + gets truncated by the lack of travel distance. Since steps are integer values and to keep + the distance traveled over the segment exact, a fractional step remaining after the last + executed step in a segment is handled by allowing the stepper algorithm distance counters + to tick to this fractional value without triggering a full step. So, when the next segment + is loaded for execution, its first full step will already have the distance counters primed + with the previous segment fractional step and will execute exactly on time according to + the planner block velocity profile. This ensures the step phasing between segments are kept + in sync and prevents artificially created accelerations between segments if they are not + accounted for. This allows the stepper algorithm to run at very high step rates without + losing steps. */ - /* - TODO: Figure out how to enforce a deceleration when a feedrate override is reduced. - The problem is that when an override is reduced, the planner may not plan back to - the current rate. Meaning that the velocity profiles for certain conditions no longer - are trapezoidal or triangular. For example, if the current block is cruising at a - nominal rate and the feedrate override is reduced, the new nominal rate will now be - lower. The velocity profile must first decelerate to the new nominal rate and then - follow on the new plan. So the remaining velocity profile will have a decelerate, - cruise, and another decelerate. + TODO: With feedrate overrides, increases to the override value will not significantly + change the planner and stepper current operation. When the value increases, we simply + need to recompute the block plan with new nominal speeds and maximum junction velocities. + However with a decreasing feedrate override, this gets a little tricky. The current block + plan is optimal, so if we try to reduce the feed rates, it may be impossible to create + a feasible plan at its current operating speed and decelerate down to zero at the end of + the buffer. We first have to enforce a deceleration to meet and intersect with the reduced + feedrate override plan. For example, if the current block is cruising at a nominal rate + and the feedrate override is reduced, the new nominal rate will now be lower. The velocity + profile must first decelerate to the new nominal rate and then follow on the new plan. Another issue is whether or not a feedrate override reduction causes a deceleration - that acts over several planner blocks. For example, say that the plan is already - heavily decelerating throughout it, reducing the feedrate will not do much to it. So, - how do we determine when to resume the new plan? How many blocks do we have to wait - until the new plan intersects with the deceleration curve? One plus though, the - deceleration will never be more than the number of blocks in the entire planner buffer, - but it theoretically can be equal to it when all planner blocks are decelerating already. + that acts over several planner blocks. For example, say that the plan is already heavily + decelerating throughout it, reducing the feedrate override will not do much to it. So, + how do we determine when to resume the new plan? One solution is to tie into the feed hold + handling code to enforce a deceleration, but check when the current speed is less than or + equal to the block maximum speed and is in an acceleration or cruising ramp. At this + point, we know that we can recompute the block velocity profile to meet and continue onto + the new block plan. */ void st_prep_buffer() { if (sys.state == STATE_QUEUED) { return; } // Block until a motion state is issued while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer. - - // Initialize new segment - st_segment_t *prep_segment = &segment_buffer[segment_buffer_head]; - prep_segment->flag = SEGMENT_NOOP; - - // Determine if we need to load a new planner block. - if (pl_prep_block == NULL) { - pl_prep_block = plan_get_block_by_index(pl_prep_index); // Query planner for a queued block - if (pl_prep_block == NULL) { return; } // No planner blocks. Exit. - - // Increment stepper common data index - if ( ++st_data_prep_index == (SEGMENT_BUFFER_SIZE-1) ) { st_data_prep_index = 0; } - - // Check if the planner has re-computed this block mid-execution. If so, push the previous segment - // data. Otherwise, prepare a new segment data for the new planner block. - if (pl_partial_block_flag) { - - // Prepare new shared segment block data and copy the relevant last segment block data. - st_data_t *last_st_prep_data; - last_st_prep_data = st_prep_data; - st_prep_data = &segment_data[st_data_prep_index]; - - st_prep_data->step_events_remaining = last_st_prep_data->step_events_remaining; - st_prep_data->rate_delta = last_st_prep_data->rate_delta; - st_prep_data->dist_per_step = last_st_prep_data->dist_per_step; - st_prep_data->nominal_rate = last_st_prep_data->nominal_rate; // TODO: Feedrate overrides recomputes this. - - st_prep_data->mm_per_step = last_st_prep_data->mm_per_step; - pl_partial_block_flag = false; // Reset flag - + // Determine if we need to load a new planner block. If so, prepare step data. + if (pl_block == NULL) { + pl_block = plan_get_current_block(); // Query planner for a queued block + if (pl_block == NULL) { return; } // No planner blocks. Exit. + +// SPINDLE_ENABLE_PORT ^= 1<step_events_remaining = pl_prep_block->step_event_count; + // Increment stepper common data index + if ( ++prep.st_block_index == (SEGMENT_BUFFER_SIZE-1) ) { prep.st_block_index = 0; } - // Convert planner block velocity profile data to stepper rate and step distance data. - st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) - st_prep_data->rate_delta = ceil(pl_prep_block->acceleration* - ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) - st_prep_data->dist_per_step = ceil((pl_prep_block->millimeters*INV_TIME_MULTIPLIER)/pl_prep_block->step_event_count); // (mult*mm/step) - - // TODO: Check if we really need to store this. - st_prep_data->mm_per_step = pl_prep_block->millimeters/pl_prep_block->step_event_count; - + // Prepare and copy Bresenham algorithm segment data from the new planner block, so that + // when the segment buffer completes the planner block, it may be discarded immediately. + st_prep_block = &st_block_buffer[prep.st_block_index]; + st_prep_block->steps[X_AXIS] = pl_block->steps[X_AXIS]; + st_prep_block->steps[Y_AXIS] = pl_block->steps[Y_AXIS]; + st_prep_block->steps[Z_AXIS] = pl_block->steps[Z_AXIS]; + st_prep_block->direction_bits = pl_block->direction_bits; + st_prep_block->step_event_count = pl_block->step_event_count; + + // Initialize planner block step count, unit distance data, and remainder tracker. + prep.step_per_mm = st_prep_block->step_event_count/pl_block->millimeters; + prep.step_events_remaining = st_prep_block->step_event_count; } - // Convert planner entry speed to stepper initial rate. - st_prep_data->initial_rate = ceil(sqrt(pl_prep_block->entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) - - // TODO: Nominal rate changes with feedrate override. - // st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) - - st_prep_data->current_approx_rate = st_prep_data->initial_rate; - - // Calculate the planner block velocity profile type, determine deceleration point, and initial ramp. - float mm_decelerate_after = plan_calculate_velocity_profile(pl_prep_index); - st_prep_data->decelerate_after = ceil( mm_decelerate_after/st_prep_data->mm_per_step ); - if (st_prep_data->decelerate_after > 0) { // If 0, RAMP_CHANGE_DECEL flag is set later. - if (st_prep_data->initial_rate != st_prep_data->nominal_rate) { prep_segment->flag = RAMP_CHANGE_ACCEL; } + // Compute velocity profile for a feed hold in-process. +// prep.current_speed = sqrt(pl_block->entry_speed_sqr); // !! For a new block, this needs to be updated. +// float inv_2_accel = 0.5/pl_block->acceleration; +// if (sys.state == STATE_HOLD) { +// prep.maximum_speed = prep.current_speed; +// prep.decelerate_after = pl_block->millimeters; +// prep.accelerate_until = pl_block->millimeters; +// prep.ramp_type = DECEL_RAMP; // or FEED_HOLD_RAMP? +// float decelerate_distance = inv_2_accel*pl_block->entry_speed_sqr; +// if (decelerate_distance > pl_block->millimeters) { +// // Keep decelerating through to the end of the block. +// // !! Need to update the next block's entry speed with current speed when loaded. +// prep.exit_speed = sqrt(pl_block->entry_speed_sqr - 2*pl_block->acceleration*pl_block->millimeters); +// } else { +// prep.exit_speed = 0.0; +// ***millimeters = decelerate_distance; // !! Need separate millimeters to track. +// // !! When target mm is reached, don't discard block until it really is at its end. +// // Return state to QUEUED and replan remaining buffer. That's about it. +// } +// } else { + + // Compute the critical velocity profile parameters of the prepped planner block. + prep.current_speed = sqrt(pl_block->entry_speed_sqr); + prep.exit_speed = plan_get_exec_block_exit_speed(); + prep.ramp_type = RAMP_ACCEL; // Initialize as acceleration ramp. + float exit_speed_sqr = prep.exit_speed*prep.exit_speed; + float inv_2_accel = 0.5/pl_block->acceleration; + float intersection_dist = + 0.5*(pl_block->millimeters+inv_2_accel*(pl_block->entry_speed_sqr-exit_speed_sqr)); + if (intersection_dist > 0.0) { + if (intersection_dist < pl_block->millimeters) { // Either trapezoid or triangle types + // NOTE: For acceleration-cruise trapezoid, following calculation will be 0.0. + prep.decelerate_after = inv_2_accel*(pl_block->nominal_speed_sqr-exit_speed_sqr); + if (prep.decelerate_after < intersection_dist) { // Trapezoid type + prep.maximum_speed = sqrt(pl_block->nominal_speed_sqr); + if (pl_block->entry_speed_sqr == pl_block->nominal_speed_sqr) { + // Cruise-deceleration or cruise-only type. + prep.ramp_type = RAMP_CRUISE; + prep.accelerate_until = pl_block->millimeters; + } else { + // Full-trapezoid or acceleration-cruise types + prep.accelerate_until = + pl_block->millimeters-inv_2_accel*(pl_block->nominal_speed_sqr-pl_block->entry_speed_sqr); + } + } else { // Triangle type + prep.accelerate_until = intersection_dist; + prep.decelerate_after = intersection_dist; + prep.maximum_speed = sqrt(2.0*pl_block->acceleration*intersection_dist+exit_speed_sqr); + } + } else { // Deceleration-only type + prep.ramp_type = RAMP_DECEL; + prep.maximum_speed = prep.current_speed; + prep.accelerate_until = pl_block->millimeters; + prep.decelerate_after = pl_block->millimeters; + } + } else { // Acceleration-only type + prep.maximum_speed = prep.exit_speed; + prep.accelerate_until = 0.0; + prep.decelerate_after = 0.0; } + } + // Initialize new segment + segment_t *prep_segment = &segment_buffer[segment_buffer_head]; + // Set new segment to point to the current segment data block. - prep_segment->st_data_index = st_data_prep_index; + prep_segment->st_block_index = prep.st_block_index; - // Approximate the velocity over the new segment using the already computed rate values. - // NOTE: This assumes that each segment will have an execution time roughly equal to every ACCELERATION_TICK. - // We do this to minimize memory and computational requirements. However, this could easily be replaced with - // a more exact approximation or have an user-defined time per segment, if CPU and memory overhead allows. - if (st_prep_data->decelerate_after <= 0) { - if (st_prep_data->decelerate_after == 0) { prep_segment->flag = RAMP_CHANGE_DECEL; } // Set segment deceleration flag - else { st_prep_data->current_approx_rate -= st_prep_data->rate_delta; } - if (st_prep_data->current_approx_rate < st_prep_data->rate_delta) { st_prep_data->current_approx_rate >>= 1; } - } else { - if (st_prep_data->current_approx_rate < st_prep_data->nominal_rate) { - st_prep_data->current_approx_rate += st_prep_data->rate_delta; - if (st_prep_data->current_approx_rate > st_prep_data->nominal_rate) { - st_prep_data->current_approx_rate = st_prep_data->nominal_rate; - } + /* ----------------------------------------------------------------------------------- + Compute the average velocity of this new segment by determining the total distance + traveled over the segment time DT_SEGMENT. The follow code first attempts to create + a full segment based on the current ramp conditions. If the segment time is incomplete + by terminating at a ramp state change, the code will continue to loop through the + progressing ramp states to fill the remaining segment execution time. However, if + an incomplete segment terminates at the end of the planner block, the segment is + considered completed despite having a truncated execution time less than DT_SEGMENT. + */ + float dt = 0.0; + float mm_remaining = pl_block->millimeters; + float time_var = DT_SEGMENT; // Time worker variable + float mm_var; // mm distance worker variable + do { + switch (prep.ramp_type) { + case RAMP_ACCEL: + // NOTE: Acceleration ramp always computes during first loop only. + mm_remaining -= DT_SEGMENT*(prep.current_speed + pl_block->acceleration*(0.5*DT_SEGMENT)); + if (mm_remaining < prep.accelerate_until) { // End of acceleration ramp. + // Acceleration-cruise, acceleration-deceleration ramp junction, or end of block. + mm_remaining = prep.accelerate_until; // NOTE: 0.0 at EOB + time_var = 2.0*(pl_block->millimeters-mm_remaining)/(prep.current_speed+prep.maximum_speed); + if (mm_remaining == prep.decelerate_after) { prep.ramp_type = RAMP_DECEL; } + else { prep.ramp_type = RAMP_CRUISE; } + prep.current_speed = prep.maximum_speed; + } else { // Acceleration only. + prep.current_speed += pl_block->acceleration*time_var; + } + break; + case RAMP_CRUISE: + // NOTE: mm_var used to retain the last mm_remaining for incomplete segment time_var calculations. + mm_var = mm_remaining - prep.maximum_speed*time_var; + if (mm_var < prep.decelerate_after) { // End of cruise. + // Cruise-deceleration junction or end of block. + time_var = (mm_remaining - prep.decelerate_after)/prep.maximum_speed; + mm_remaining = prep.decelerate_after; // NOTE: 0.0 at EOB + prep.ramp_type = RAMP_DECEL; + } else { // Cruising only. + mm_remaining = mm_var; + } + break; + default: // case RAMP_DECEL: + // NOTE: mm_var used to catch negative decelerate distance values near zero speed. + mm_var = time_var*(prep.current_speed - 0.5*pl_block->acceleration*time_var); + if ((mm_var > 0.0) && (mm_var < mm_remaining)) { // Deceleration only. + prep.current_speed -= pl_block->acceleration*time_var; + // Check for near-zero speed and prevent divide by zero in rare scenarios. + if (prep.current_speed > prep.exit_speed) { mm_remaining -= mm_var; } + else { mm_remaining = 0.0; } // NOTE: Force EOB for now. May change later. + } else { // End of block. + time_var = 2.0*mm_remaining/(prep.current_speed+prep.exit_speed); + mm_remaining = 0.0; + // prep.current_speed = prep.exit_speed; + } } - } - - // TODO: Look into replacing the following dist_per_step divide with multiplying its inverse to save cycles. - - // Compute the number of steps in the prepped segment based on the approximate current rate. - // NOTE: The dist_per_step divide cancels out the INV_TIME_MULTIPLIER and converts the rate value to steps. - prep_segment->n_step = ceil(max(MINIMUM_STEP_RATE,st_prep_data->current_approx_rate)* - (ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND)/st_prep_data->dist_per_step); - // NOTE: Ensures it moves for very slow motions, but MINIMUM_STEP_RATE should always set this too. Perhaps - // a compile-time check to see if MINIMUM_STEP_RATE is set high enough is all that is needed. - prep_segment->n_step = max(prep_segment->n_step,MINIMUM_STEPS_PER_SEGMENT); - // NOTE: As long as the ACCELERATION_TICKS_PER_SECOND is valid, n_step should never exceed 255 and overflow. - // prep_segment->n_step = min(prep_segment->n_step,MAXIMUM_STEPS_PER_BLOCK); // Prevent unsigned int8 overflow. - - // Check if n_step exceeds steps remaining in planner block. If so, truncate. - if (prep_segment->n_step > st_prep_data->step_events_remaining) { - prep_segment->n_step = st_prep_data->step_events_remaining; + dt += time_var; // Add computed ramp time to total segment time. + if (dt < DT_SEGMENT) { time_var = DT_SEGMENT - dt; } // **Incomplete** At ramp junction. + else { break; } // **Complete** Exit loop. Segment execution time maxed. + } while ( mm_remaining > 0.0 ); // **Complete** Exit loop. End of planner block. + + /* ----------------------------------------------------------------------------------- + Compute segment step rate, steps to execute, and step phase correction parameters. + NOTE: Steps are computed by direct scalar conversion of the millimeter distance + remaining in the block, rather than incrementally tallying the steps executed per + segment. This helps in removing floating point round-off issues of several additions. + However, since floats have only 7.2 significant digits, long moves with extremely + high step counts can exceed the precision of floats, which can lead to lost steps. + Fortunately, this scenario is highly unlikely and unrealistic in CNC machines + supported by Grbl (i.e. exceeding 10 meters axis travel at 200 step/mm). + */ + // Use time_var to pre-compute dt inversion with integer multiplier. + time_var = (INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))/dt; // (mult/isr_tic) + if (mm_remaining > 0.0) { + float steps_remaining = prep.step_per_mm*mm_remaining; + prep_segment->dist_per_tick = ceil( (prep.step_events_remaining-steps_remaining)*time_var ); // (mult*step/isr_tic) + + // Compute number of steps to execute and segment step phase correction. + prep_segment->phase_dist = ceil(INV_TIME_MULTIPLIER*(ceil(steps_remaining)-steps_remaining)); + prep_segment->n_step = ceil(prep.step_events_remaining)-ceil(steps_remaining); + + // Update step execution variables + pl_block->millimeters = mm_remaining; + prep.step_events_remaining = steps_remaining; + + } else { // End of block. + // Set to execute the remaining steps and no phase correction upon finishing the block. + prep_segment->dist_per_tick = ceil( prep.step_events_remaining*time_var ); // (mult*step/isr_tic) + prep_segment->phase_dist = 0; + prep_segment->n_step = ceil(prep.step_events_remaining); + + // The planner block is complete. All steps are set to be executed in the segment buffer. + // TODO: Ignore this for feed holds. Need to recalculate the planner buffer at this time. + pl_block = NULL; + plan_discard_current_block(); } - // Check if n_step crosses decelerate point in block. If so, truncate to ensure the deceleration - // ramp counters are set correctly during execution. - if (st_prep_data->decelerate_after > 0) { - if (prep_segment->n_step > st_prep_data->decelerate_after) { - prep_segment->n_step = st_prep_data->decelerate_after; - } - } - - // Update stepper common data variables. - st_prep_data->decelerate_after -= prep_segment->n_step; - st_prep_data->step_events_remaining -= prep_segment->n_step; - - // Check for end of planner block - if ( st_prep_data->step_events_remaining == 0 ) { - - // TODO: When a feed hold ends, the step_events_remaining will also be zero, even though a block - // have partially been completed. We need to flag the stepper algorithm to indicate a stepper shutdown - // when complete, but not remove the planner block unless it truly is the end of the block (rare). - - // Set EOB bitflag so stepper algorithm discards the planner block after this segment completes. - prep_segment->flag |= SEGMENT_END_OF_BLOCK; - // Move planner pointer to next block and flag to load a new block for the next segment. - pl_prep_index = plan_next_block_index(pl_prep_index); - pl_prep_block = NULL; - } - - // New step segment completed. Increment segment buffer indices. + // New step segment initialization completed. Increment segment buffer indices. segment_buffer_head = segment_next_head; if ( ++segment_next_head == SEGMENT_BUFFER_SIZE ) { segment_next_head = 0; } - -// long a = prep_segment->n_step; -// printInteger(a); -// printString(" "); +// int32_t blength = segment_buffer_head - segment_buffer_tail; +// if (blength < 0) { blength += SEGMENT_BUFFER_SIZE; } +// printInteger(blength); +// SPINDLE_ENABLE_PORT ^= 1<step_events_remaining*st_prep_data->mm_per_step; - if (st_prep_data->decelerate_after > 0) { *is_decelerating = false; } - else { *is_decelerating = true; } - - // Flag for new prep_block when st_prep_buffer() is called after the planner recomputes. - pl_partial_block_flag = true; - pl_prep_block = NULL; - } - return; -} diff --git a/stepper.h b/stepper.h index 8e2a6e0..e2187b2 100644 --- a/stepper.h +++ b/stepper.h @@ -3,7 +3,7 @@ Part of Grbl Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2011 Sungeun K. Jeon + Copyright (c) 2011-2013 Sungeun K. 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 @@ -24,6 +24,10 @@ #include +#ifndef SEGMENT_BUFFER_SIZE + #define SEGMENT_BUFFER_SIZE 7 +#endif + // Initialize and setup the stepper motor subsystem void st_init(); @@ -45,10 +49,10 @@ void st_cycle_reinitialize(); // Initiates a feed hold of the running program void st_feed_hold(); +// Reloads step segment buffer. Called continuously by runtime execution protocol. void st_prep_buffer(); -uint8_t st_get_prep_block_index(); - -void st_fetch_partial_block_parameters(uint8_t block_index, float *millimeters_remaining, uint8_t *is_decelerating); +// Called by planner_recalculate() when the executing block is updated by the new plan. +void st_update_plan_block_parameters(); #endif diff --git a/stepper_test.c b/stepper_test.c new file mode 100644 index 0000000..fea6315 --- /dev/null +++ b/stepper_test.c @@ -0,0 +1,788 @@ +/* + stepper.c - stepper motor driver: executes motion plans using stepper motors + Part of Grbl + + Copyright (c) 2011-2013 Sungeun K. Jeon + Copyright (c) 2009-2011 Simen Svale Skogsrud + + 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 + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#include +#include "stepper.h" +#include "config.h" +#include "settings.h" +#include "planner.h" +#include "nuts_bolts.h" + +// Some useful constants +#define TICKS_PER_MICROSECOND (F_CPU/1000000) + +#define RAMP_NOOP_CRUISE 0 +#define RAMP_ACCEL 1 +#define RAMP_DECEL 2 + +#define LOAD_NOOP 0 +#define LOAD_SEGMENT 1 +#define LOAD_BLOCK 2 + +#define SEGMENT_NOOP 0 +#define SEGMENT_END_OF_BLOCK bit(0) +#define RAMP_CHANGE_ACCEL bit(1) +#define RAMP_CHANGE_DECEL bit(2) + +#define MINIMUM_STEPS_PER_SEGMENT 1 // Don't change + +#define SEGMENT_BUFFER_SIZE 6 + +#define DT_SEGMENT (1/ACCELERATION_TICKS_PER_SECOND) + +// Stepper state variable. Contains running data and trapezoid variables. +typedef struct { + // Used by the bresenham line algorithm + int32_t counter_x, // Counter variables for the bresenham line tracer + counter_y, + counter_z; + + // Used by inverse time algorithm to track step rate + int32_t counter_dist; // Inverse time distance traveled since last step event + + uint8_t step_count; // Steps remaining in line segment motion + uint8_t phase_count; // Phase ticks remaining after line segment steps complete + + // Used by the stepper driver interrupt + uint8_t execute_step; // Flags step execution for each interrupt. + uint8_t step_pulse_time; // Step pulse reset time after step rise + uint8_t out_bits; // The next stepping-bits to be output + uint8_t load_flag; +} stepper_t; +static stepper_t st; + +// Stores stepper common data for executing steps in the segment buffer. Data can change mid-block when the +// planner updates the remaining block velocity profile with a more optimal plan or a feedrate override occurs. +// NOTE: Normally, this buffer is partially in-use, but, for the worst case scenario, it will never exceed +// the number of accessible stepper buffer segments (SEGMENT_BUFFER_SIZE-1). +typedef struct { + + // TODO: Retain step[N_AXIS], step_event_count, and direction byte here, so that we can throw + // away the planner block when the segment prep is complete. + + float step_events_remaining; // Tracks step event count for the executing planner block + + // Planner block velocity profile parameters used to trace and execute steps.; + float accelerate_until; + float decelerate_after; + float current_speed; + float maximum_speed; + float exit_speed; +} st_data_t; +static st_data_t segment_data[SEGMENT_BUFFER_SIZE-1]; + +// Primary stepper segment ring buffer. Contains small, short line segments for the stepper algorithm to execute, +// which are "checked-out" incrementally from the first block in the planner buffer. Once "checked-out", the steps +// in the segments buffer cannot be modified by the planner, where the remaining planner block steps still can. +typedef struct { + uint8_t n_step; // Number of step events to be executed for this segment + uint8_t n_phase_tick; + uint32_t dist_per_tick; + uint8_t st_data_index; // Stepper buffer common data index. Uses this information to execute this segment. + uint8_t flag; // Stepper algorithm bit-flag for special execution conditions. +} st_segment_t; +static st_segment_t segment_buffer[SEGMENT_BUFFER_SIZE]; + +// Step segment ring buffer indices +static volatile uint8_t segment_buffer_tail; +static volatile uint8_t segment_buffer_head; +static uint8_t segment_next_head; + +static volatile uint8_t busy; // Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though. +static plan_block_t *pl_current_block; // A pointer to the planner block currently being traced +static st_segment_t *st_current_segment; +static st_data_t *st_current_data; + +// Pointers for the step segment being prepped from the planner buffer. Accessed only by the +// main program. Pointers may be planning segments or planner blocks ahead of what being executed. +static plan_block_t *pl_prep_block; // Pointer to the planner block being prepped +static st_data_t *st_prep_data; // Pointer to the stepper common data being prepped +static uint8_t pl_prep_index; // Index of planner block being prepped +static uint8_t st_data_prep_index; // Index of stepper common data block being prepped +static uint8_t pl_partial_block_flag; // Flag indicating the planner has modified the prepped planner block + +static float st_prep_step_per_mm; + +// TODO: All this stuff only needs to be retained for the prepped planner block. Once changed +// or complete, we do not need this information anymore. Duh! +// typedef struct { +// float step_events_remaining; // Tracks step event count for the executing planner block +// float accelerate_until; +// float decelerate_after; +// float current_speed; +// float maximum_speed; +// float exit_speed; +// float step_per_mm; +// } st_prep_data_t; + +/* __________________________ + /| |\ _________________ ^ + / | | \ /| |\ | + / | | \ / | | \ s + / | | | | | \ p + / | | | | | \ e + +-----+------------------------+---+--+---------------+----+ e + | BLOCK 1 | BLOCK 2 | d + + time -----> + + The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta + until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after + after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as + +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. +*/ + +// Stepper state initialization. Cycle should only start if the st.cycle_start flag is +// enabled. Startup init and limits call this function but shouldn't start the cycle. +void st_wake_up() +{ + // Enable steppers by resetting the stepper disable port + if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { + STEPPERS_DISABLE_PORT |= (1<> 3); + // Enable stepper driver interrupt + st.execute_step = false; + st.load_flag = LOAD_BLOCK; + + TCNT2 = 0; // Clear Timer2 + TIMSK2 |= (1<n_step; + + // If the new segment starts a new planner block, initialize stepper variables and counters. + // NOTE: For new segments only, the step counters are not updated to ensure step phasing is continuous. + if (st.load_flag == LOAD_BLOCK) { + pl_current_block = plan_get_current_block(); // Should always be there. Stepper buffer handles this. + st_current_data = &segment_data[segment_buffer[segment_buffer_tail].st_data_index]; + + // Initialize direction bits for block. Set execute flag to set directions bits upon next ISR tick. + st.out_bits = pl_current_block->direction_bits ^ settings.invert_mask; + st.execute_step = true; + + // Initialize Bresenham line counters + st.counter_x = (pl_current_block->step_event_count >> 1); + st.counter_y = st.counter_x; + st.counter_z = st.counter_x; + + // Initialize inverse time, step rate data, and acceleration ramp counters + st.counter_dist = INV_TIME_MULTIPLIER; // dist_per_step always greater than dist_per_tick. + } + + st.load_flag = LOAD_NOOP; // Segment motion loaded. Set no-operation flag to skip during execution. + + } else { + // Can't discard planner block here if a feed hold stops in middle of block. + st_go_idle(); + bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end + return; // Nothing to do but exit. + } + + } + + // Iterate inverse time counter. Triggers each Bresenham step event. + st.counter_dist -= st_current_segment->dist_per_tick; + + // Execute Bresenham step event, when it's time to do so. + if (st.counter_dist < 0) { + if (st.step_count > 0) { // Block phase correction from executing step. + st.counter_dist += INV_TIME_MULTIPLIER; // Reload inverse time counter + + st.out_bits = pl_current_block->direction_bits; // Reset out_bits and reload direction bits + st.execute_step = true; + + // Execute step displacement profile by Bresenham line algorithm + st.counter_x -= pl_current_block->steps[X_AXIS]; + if (st.counter_x < 0) { + st.out_bits |= (1<step_event_count; + if (st.out_bits & (1<steps[Y_AXIS]; + if (st.counter_y < 0) { + st.out_bits |= (1<step_event_count; + if (st.out_bits & (1<steps[Z_AXIS]; + if (st.counter_z < 0) { + st.out_bits |= (1<step_event_count; + if (st.out_bits & (1<flag & SEGMENT_END_OF_BLOCK) { + plan_discard_current_block(); + st.load_flag = LOAD_BLOCK; + } else { + st.load_flag = LOAD_SEGMENT; + } + + // Discard current segment by advancing buffer tail index + if ( ++segment_buffer_tail == SEGMENT_BUFFER_SIZE) { segment_buffer_tail = 0; } + } + st.phase_count--; + } + + + busy = false; +// SPINDLE_ENABLE_PORT ^= 1<step_events_remaining); +// st.ramp_type = RAMP_ACCEL; +// st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK/2; +// st.ramp_rate = 0; +// sys.state = STATE_QUEUED; +// } else { +// sys.state = STATE_IDLE; +// } + sys.state = STATE_IDLE; + +} + + +/* Prepares step segment buffer. Continuously called from main program. + + The segment buffer is an intermediary buffer interface between the execution of steps + by the stepper algorithm and the velocity profiles generated by the planner. The stepper + algorithm only executes steps within the segment buffer and is filled by the main program + when steps are "checked-out" from the first block in the planner buffer. This keeps the + step execution and planning optimization processes atomic and protected from each other. + The number of steps "checked-out" from the planner buffer and the number of segments in + the segment buffer is sized and computed such that no operation in the main program takes + longer than the time it takes the stepper algorithm to empty it before refilling it. + Currently, the segment buffer conservatively holds roughly up to 40-60 msec of steps. + + NOTE: The segment buffer executes a set number of steps over an approximate time period. + If we try to execute over a fixed time period, it is difficult to guarantee or predict + how many steps will execute over it, especially when the step pulse phasing between the + neighboring segments must also be kept consistent. Meaning that, if the last segment step + pulses right before a segment end, the next segment must delay its first pulse so that the + step pulses are consistently spaced apart over time to keep the step pulse train nice and + smooth. Keeping track of phasing and ensuring that the exact number of steps are executed + as defined by the planner block, the related computational overhead can get quickly and + prohibitively expensive, especially in real-time. + Since the stepper algorithm automatically takes care of the step pulse phasing with + its ramp and inverse time counters by retaining the count remainders, we don't have to + explicitly and expensively track and synchronize the exact number of steps, time, and + phasing of steps. All we need to do is approximate the number of steps in each segment + such that the segment buffer has enough execution time for the main program to do what + it needs to do and refill it when it comes back. In other words, we just need to compute + a cheap approximation of the current velocity and the number of steps over it. +*/ + +/* + TODO: Figure out how to enforce a deceleration when a feedrate override is reduced. + The problem is that when an override is reduced, the planner may not plan back to + the current rate. Meaning that the velocity profiles for certain conditions no longer + are trapezoidal or triangular. For example, if the current block is cruising at a + nominal rate and the feedrate override is reduced, the new nominal rate will now be + lower. The velocity profile must first decelerate to the new nominal rate and then + follow on the new plan. So the remaining velocity profile will have a decelerate, + cruise, and another decelerate. + Another issue is whether or not a feedrate override reduction causes a deceleration + that acts over several planner blocks. For example, say that the plan is already + heavily decelerating throughout it, reducing the feedrate will not do much to it. So, + how do we determine when to resume the new plan? How many blocks do we have to wait + until the new plan intersects with the deceleration curve? One plus though, the + deceleration will never be more than the number of blocks in the entire planner buffer, + but it theoretically can be equal to it when all planner blocks are decelerating already. +*/ +void st_prep_buffer() +{ + if (sys.state == STATE_QUEUED) { return; } // Block until a motion state is issued + while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer. + + // Initialize new segment + st_segment_t *prep_segment = &segment_buffer[segment_buffer_head]; + prep_segment->flag = SEGMENT_NOOP; + + // ----------------------------------------------------------------------------------- + // Determine if we need to load a new planner block. If so, prepare step data. + if (pl_prep_block == NULL) { + pl_prep_block = plan_get_block_by_index(pl_prep_index); // Query planner for a queued block + if (pl_prep_block == NULL) { return; } // No planner blocks. Exit. + + // Increment stepper common data index + if ( ++st_data_prep_index == (SEGMENT_BUFFER_SIZE-1) ) { st_data_prep_index = 0; } + + // Check if the planner has re-computed this block mid-execution. If so, push the previous + // segment data. Otherwise, prepare a new segment data for the new planner block. + if (pl_partial_block_flag) { + + // Prepare new shared segment block data and copy the relevant last segment block data. + st_data_t *last_st_prep_data; + last_st_prep_data = st_prep_data; + st_prep_data = &segment_data[st_data_prep_index]; + + st_prep_data->step_events_remaining = last_st_prep_data->step_events_remaining; + + pl_partial_block_flag = false; // Reset flag + + } else { + + // Prepare commonly shared planner block data for the ensuing segment buffer moves ad-hoc, since + // the planner buffer can dynamically change the velocity profile data as blocks are added. + st_prep_data = &segment_data[st_data_prep_index]; + + st_prep_step_per_mm = pl_prep_block->step_event_count/pl_prep_block->millimeters; + + // Initialize planner block step data + st_prep_data->step_events_remaining = pl_prep_block->step_event_count; + + } + + + st_prep_data->current_speed = sqrt(pl_prep_block->entry_speed_sqr); + + // Determine current block exit speed + plan_block_t *pl_next_block = plan_get_block_by_index(plan_next_block_index(pl_prep_index)); + float exit_speed_sqr; + if (pl_next_block == NULL) { + exit_speed_sqr = 0.0; + st_prep_data->exit_speed = 0.0; + } else { + exit_speed_sqr = pl_next_block->entry_speed_sqr; + st_prep_data->exit_speed = sqrt(exit_speed_sqr); + } + +// st_prep_data->accelerate_until = 0.5*(pl_prep_block->nominal_speed_sqr-pl_prep_block->entry_speed_sqr)/(pl_prep_block->acceleration); +// st_prep_data->decelerate_after = 0.5*(pl_prep_block->nominal_speed_sqr-exit_speed_sqr)/(pl_prep_block->acceleration); +// if (pl_prep_block->millimeters < st_prep_data->accelerate_until+st_prep_data->decelerate_after) { +// st_prep_data->decelerate_after = 0.5*( pl_prep_block->millimeters + 0.5*(pl_prep_block->entry_speed_sqr +// - exit_speed_sqr)/(pl_prep_block->acceleration) ); +// st_prep_data->accelerate_until = pl_prep_block->millimeters-st_prep_data->decelerate_after; +// st_prep_data->maximum_speed = sqrt(2*pl_prep_block->acceleration*st_prep_data->decelerate_after+exit_speed_sqr); +// } else { +// st_prep_data->accelerate_until = pl_prep_block->millimeters-st_prep_data->accelerate_until; +// st_prep_data->maximum_speed = sqrt(pl_prep_block->nominal_speed_sqr); +// } + + // Determine velocity profile based on the 7 possible types: Cruise-only, cruise-deceleration, + // acceleration-cruise, acceleration-only, deceleration-only, trapezoid, and triangle. + st_prep_data->accelerate_until = pl_prep_block->millimeters; + if (pl_prep_block->entry_speed_sqr == pl_prep_block->nominal_speed_sqr) { + st_prep_data->maximum_speed = sqrt(pl_prep_block->nominal_speed_sqr); + if (exit_speed_sqr == pl_prep_block->nominal_speed_sqr) { // Cruise-only type + st_prep_data->decelerate_after = 0.0; + } else { // Cruise-deceleration type + st_prep_data->decelerate_after = 0.5*(pl_prep_block->nominal_speed_sqr-exit_speed_sqr)/(pl_prep_block->acceleration); + } + } else if (exit_speed_sqr == pl_prep_block->nominal_speed_sqr) { + // Acceleration-cruise type + st_prep_data->maximum_speed = sqrt(pl_prep_block->nominal_speed_sqr); + st_prep_data->decelerate_after = 0.0; + st_prep_data->accelerate_until -= 0.5*(pl_prep_block->nominal_speed_sqr-pl_prep_block->entry_speed_sqr)/(pl_prep_block->acceleration); + } else { + float intersection_dist = 0.5*( pl_prep_block->millimeters + 0.5*(pl_prep_block->entry_speed_sqr + - exit_speed_sqr)/(pl_prep_block->acceleration) ); + if (intersection_dist > 0.0) { + if (intersection_dist < pl_prep_block->millimeters) { // Either trapezoid or triangle types + st_prep_data->decelerate_after = 0.5*(pl_prep_block->nominal_speed_sqr-exit_speed_sqr)/(pl_prep_block->acceleration); + if (st_prep_data->decelerate_after < intersection_dist) { // Trapezoid type + st_prep_data->maximum_speed = sqrt(pl_prep_block->nominal_speed_sqr); + st_prep_data->accelerate_until -= 0.5*(pl_prep_block->nominal_speed_sqr-pl_prep_block->entry_speed_sqr)/(pl_prep_block->acceleration); + } else { // Triangle type + st_prep_data->decelerate_after = intersection_dist; + st_prep_data->maximum_speed = sqrt(2*pl_prep_block->acceleration*st_prep_data->decelerate_after+exit_speed_sqr); + st_prep_data->accelerate_until -= st_prep_data->decelerate_after; + } + } else { // Deceleration-only type + st_prep_data->maximum_speed = st_prep_data->current_speed; + st_prep_data->decelerate_after = pl_prep_block->millimeters; + } + } else { // Acceleration-only type + st_prep_data->maximum_speed = st_prep_data->exit_speed; + st_prep_data->decelerate_after = 0.0; + st_prep_data->accelerate_until = 0.0; + } + } + } + + // Set new segment to point to the current segment data block. + prep_segment->st_data_index = st_data_prep_index; + + // ----------------------------------------------------------------------------------- + // Initialize segment execute distance. Attempt to create a full segment over DT_SEGMENT. + + float mm_remaining = pl_prep_block->millimeters; + float dt = DT_SEGMENT; + if (mm_remaining > st_prep_data->accelerate_until) { // Acceleration ramp + mm_remaining -= (st_prep_data->current_speed*DT_SEGMENT + + pl_prep_block->acceleration*(0.5*DT_SEGMENT*DT_SEGMENT)); + if (mm_remaining < st_prep_data->accelerate_until) { // **Incomplete** Acceleration ramp end. + // Acceleration-cruise, acceleration-deceleration ramp junction, or end of block. + mm_remaining = st_prep_data->accelerate_until; // NOTE: 0.0 at EOB + dt = 2*(pl_prep_block->millimeters-mm_remaining)/ + (st_prep_data->current_speed+st_prep_data->maximum_speed); + st_prep_data->current_speed = st_prep_data->maximum_speed; + } else { // **Complete** Acceleration only. + st_prep_data->current_speed += pl_prep_block->acceleration*DT_SEGMENT; + } + } else if (mm_remaining <= st_prep_data->decelerate_after) { // Deceleration ramp + mm_remaining -= (st_prep_data->current_speed*DT_SEGMENT + - pl_prep_block->acceleration*(0.5*DT_SEGMENT*DT_SEGMENT)); + if (mm_remaining > 0.0) { // **Complete** Deceleration only. + st_prep_data->current_speed -= pl_prep_block->acceleration*DT_SEGMENT; + } else { // **Complete* End of block. + dt = 2*pl_prep_block->millimeters/(st_prep_data->current_speed+st_prep_data->exit_speed); + mm_remaining = 0.0; + // st_prep_data->current_speed = st_prep_data->exit_speed; + } + } else { // Cruising profile + mm_remaining -= st_prep_data->maximum_speed*DT_SEGMENT; + if (mm_remaining < st_prep_data->decelerate_after) { // **Incomplete** End of cruise. + // Cruise-deceleration junction or end of block. + mm_remaining = st_prep_data->decelerate_after; // NOTE: 0.0 at EOB + dt = (pl_prep_block->millimeters-mm_remaining)/st_prep_data->maximum_speed; + } // Otherwise **Complete** Cruising only. + } + + // ----------------------------------------------------------------------------------- + // If segment is incomplete, attempt to fill the remainder. + // NOTE: Segment remainder always spans a cruise and/or a deceleration ramp. + + if (dt < DT_SEGMENT) { + if (mm_remaining > 0.0) { // Skip if end of block. + float last_mm_remaining; + float dt_remainder; + + // Fill incomplete segment with an acceleration junction. + if (mm_remaining > st_prep_data->decelerate_after) { // Cruising profile + last_mm_remaining = mm_remaining; + dt_remainder = DT_SEGMENT-dt; + mm_remaining -= st_prep_data->current_speed*dt_remainder; + if (mm_remaining < st_prep_data->decelerate_after) { // **Incomplete** + mm_remaining = st_prep_data->decelerate_after; + dt += (last_mm_remaining-mm_remaining)/st_prep_data->maximum_speed; + // current_speed = maximum_speed; + } else { // **Complete** Segment filled. + dt = DT_SEGMENT; + } + } + + // Fill incomplete segment with a deceleration junction. + if (mm_remaining > 0.0) { + if (mm_remaining <= st_prep_data->decelerate_after) { // Deceleration ramp + last_mm_remaining = mm_remaining; + dt_remainder = DT_SEGMENT-dt; + mm_remaining -= (dt_remainder*(st_prep_data->current_speed + - 0.5*pl_prep_block->acceleration*dt_remainder)); + if (mm_remaining > 0.0) { // **Complete** Segment filled. + st_prep_data->current_speed -= pl_prep_block->acceleration*dt_remainder; + dt = DT_SEGMENT; + } else { // **Complete** End of block. + mm_remaining = 0.0; + dt += (2*last_mm_remaining/(st_prep_data->current_speed+st_prep_data->exit_speed)); + // st_prep_data->current_speed = st_prep_data->exit_speed; + } + } + } + + } + } + + // ----------------------------------------------------------------------------------- + // Compute segment step rate, steps to execute, and step phase correction parameters. + + // Convert segment distance in terms of steps. +// float dist_travel = pl_prep_block->millimeters; +// if (mm_remaining > 0.0) { dist_travel -= mm_remaining; } + + + if (mm_remaining > 0.0) { + + float steps_remaining = st_prep_step_per_mm*mm_remaining; + prep_segment->dist_per_tick = ceil( (INV_TIME_MULTIPLIER/ISR_TICKS_PER_SECOND)* + (st_prep_data->step_events_remaining-steps_remaining)/dt ); // (mult*step/isr_tic) + + // Compute number of steps to execute and segment step phase correction. + prep_segment->n_step = ceil(st_prep_data->step_events_remaining)-ceil(steps_remaining); + prep_segment->n_phase_tick = ceil(INV_TIME_MULTIPLIER*(ceil(steps_remaining)-steps_remaining)/prep_segment->dist_per_tick); + + // Update step execution variables + st_prep_data->step_events_remaining = steps_remaining; + pl_prep_block->millimeters = mm_remaining; + + } else { // End of block. Finish it out. + + prep_segment->dist_per_tick = ceil( (INV_TIME_MULTIPLIER/ISR_TICKS_PER_SECOND)* + st_prep_data->step_events_remaining/dt ); // (mult*step/isr_tic) + + // Set to execute the remaining steps and no phase correction upon finishing the block. + prep_segment->n_step = ceil(st_prep_data->step_events_remaining); + prep_segment->n_phase_tick = 0; + + // NOTE: Not required. Planner will ignore this block as it is now complete. + // st_prep_data->step_events_remaining = 0.0; + // pl_prep_block->millimeters = 0.0; + + // Move planner pointer to next block and flag to load a new block for the next segment. + pl_prep_index = plan_next_block_index(pl_prep_index); + pl_prep_block = NULL; + prep_segment->flag |= SEGMENT_END_OF_BLOCK; + + } + + // !!! PROBLEM. Step events remaining in floating point can limit the number of steps + // we can accurately track, since floats have ~7.2 significant digits. However, this only + // becomes a problem if there are more than 1,000,000, which translates to a CNC machine + // with 200 step/mm and 5 meters of axis travel. Possible but unlikely. Could have more + // issues with user setting up their machine with too high of steps. + + // TODO: dist_per_tick must be less than INV_TIME_MULTIPLIER. A check can be made to + // make this a hard limit. Need to make sure this doesn't affect the velocity profiles.. + // it shouldn't. The same could said for the minimum allowable step rate too. This should + // not affect the tracing of the profiles either. + + // Ensure the initial step rate exceeds the MINIMUM_STEP_RATE. + // TODO: Use config.h error checking to do this. Otherwise, counters get screwy. + + // New step segment initialization completed. Increment segment buffer indices. + segment_buffer_head = segment_next_head; + if ( ++segment_next_head == SEGMENT_BUFFER_SIZE ) { segment_next_head = 0; } + + SPINDLE_ENABLE_PORT ^= 1<entry_speed_sqr = st_prep_data->current_speed*st_prep_data->current_speed; + // pl_partial_block->max_entry_speed_sqr = pl_partial_block->entry_speed_sqr; // Not sure if this needs to be updated. + + // Flag for new prep_block when st_prep_buffer() is called after the planner recomputes. + pl_partial_block_flag = true; + pl_prep_block = NULL; + } + return; +} From 2f6663a0b9d983ba857e400997f91a6fd93abe30 Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Wed, 4 Dec 2013 21:49:24 -0700 Subject: [PATCH 23/73] Reinstated feed holds into new stepper algorithm and planner. Rough draft, but working. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Reinstated the feed hold feature with the new stepper algorithm and new optimized planner. It works, but will be re-factored a bit soon to clean up the code. - At this point, feedrate overrides may need to be installed in the v1.0 version of grbl, while this version will likely be pushed to the edge branch soon and pushed to master after the bugs have been squashed. - Measured the overall performance of the new planner and stepper algorithm on an oscilloscope. The new planner is about 4x faster than before, where it is completing a plan in around 1ms. The stepper algorithm itself is minutely faster, as it is a little lighter. The trade-off in the increased planner performance comes from the new step segment buffer. However, even in the worse case scenario, the step segment buffer generates a new segment with a typical 0.2 ms, and the worse case is 1ms upon a new block or replanning the active block. Added altogether, it’s argubly still twice as efficient as the old one. --- planner.c | 17 +---- planner.h | 4 +- stepper.c | 218 ++++++++++++++++++++++++++++-------------------------- 3 files changed, 116 insertions(+), 123 deletions(-) diff --git a/planner.c b/planner.c index 5b2f5cf..d4a38ee 100644 --- a/planner.c +++ b/planner.c @@ -399,10 +399,6 @@ void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) // Finish up by recalculating the plan with the new block. planner_recalculate(); - -// int32_t blength = block_buffer_head - block_buffer_tail; -// if (blength < 0) { blength += BLOCK_BUFFER_SIZE; } -// printInteger(blength); } @@ -418,19 +414,10 @@ void plan_sync_position() // Re-initialize buffer plan with a partially completed block, assumed to exist at the buffer tail. // Called after a steppers have come to a complete stop for a feed hold and the cycle is stopped. -void plan_cycle_reinitialize(int32_t step_events_remaining) +void plan_cycle_reinitialize() { - plan_block_t *block = &block_buffer[block_buffer_tail]; // Point to partially completed block - - // Only remaining millimeters and step_event_count need to be updated for planner recalculate. - // Other variables (step_x, step_y, step_z, rate_delta, etc.) all need to remain the same to - // ensure the original planned motion is resumed exactly. - block->millimeters = (block->millimeters*step_events_remaining)/block->step_event_count; - block->step_event_count = step_events_remaining; - // Re-plan from a complete stop. Reset planner entry speeds and buffer planned pointer. - block->entry_speed_sqr = 0.0; - block->max_entry_speed_sqr = 0.0; +// st_update_plan_block_parameters(); block_buffer_planned = block_buffer_tail; planner_recalculate(); } diff --git a/planner.h b/planner.h index 23f254a..a1335aa 100644 --- a/planner.h +++ b/planner.h @@ -46,7 +46,7 @@ typedef struct { float nominal_speed_sqr; // Axis-limit adjusted nominal speed for this block in (mm/min)^2 float acceleration; // Axis-limit adjusted line acceleration in (mm/min^2) float millimeters; // The remaining distance for this block to be executed in (mm) - + // uint8_t max_override; // Maximum override value based on axis speed limits } plan_block_t; @@ -75,7 +75,7 @@ float plan_get_exec_block_exit_speed(); void plan_sync_position(); // Reinitialize plan with a partially completed block -void plan_cycle_reinitialize(int32_t step_events_remaining); +void plan_cycle_reinitialize(); // Returns the status of the block ring buffer. True, if buffer is full. uint8_t plan_check_full_buffer(); diff --git a/stepper.c b/stepper.c index ea78b2e..771480d 100644 --- a/stepper.c +++ b/stepper.c @@ -100,13 +100,15 @@ static st_block_t *st_prep_block; // Pointer to the stepper block data being pr // Segment preparation data struct. Contains all the necessary information to compute new segments // based on the current executing planner block. typedef struct { - uint8_t st_block_index; // Index of stepper common data block being prepped - uint8_t partial_block_flag; // Flag indicating the planner has modified the prepped planner block + uint8_t st_block_index; // Index of stepper common data block being prepped + uint8_t flag_partial_block; // Flag indicating the last block completed. Time to load a new one. float step_per_mm; // Current planner block step/millimeter conversion scalar - float step_events_remaining; // Tracks step event count for the executing planner block + float steps_remaining; + int32_t step_events_remaining; // Tracks step event count for the executing planner block uint8_t ramp_type; // Current segment ramp state + float millimeters_remaining; float current_speed; // Current speed at the end of the segment buffer (mm/min) float maximum_speed; // Maximum speed of executing block. Not always nominal speed. (mm/min) float exit_speed; // Exit speed of executing block (mm/min) @@ -217,7 +219,7 @@ void st_go_idle() three stepper outputs simultaneously with these two interrupts. */ /* TODO: - - Measure time in ISR. Typical and worst-case. Should be virtually identical to last algorithm. + - Measured time in ISR. Typical and worst-case. Roughly 5usec min to 25usec max. Good enough. There are no major changes to the base operations of this ISR with the new segment buffer. - Determine if placing the position counters elsewhere (or change them to 8-bit variables that are added to the system position counters at the end of a segment) frees up cycles. @@ -345,7 +347,7 @@ void st_reset() memset(&st, 0, sizeof(st)); st.exec_segment = NULL; pl_block = NULL; // Planner block pointer used by segment buffer - + segment_buffer_tail = 0; segment_buffer_head = 0; // empty = tail segment_next_head = 1; @@ -411,17 +413,9 @@ void st_feed_hold() // Only the planner de/ac-celerations profiles and stepper rates have been updated. void st_cycle_reinitialize() { -// if (pl_current_block != NULL) { -// plan_cycle_reinitialize(st_exec_block->step_events_remaining); -// st.ramp_type = RAMP_ACCEL; -// st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK/2; -// st.ramp_rate = 0; -// sys.state = STATE_QUEUED; -// } else { -// sys.state = STATE_IDLE; -// } + if (sys.state != STATE_QUEUED) { sys.state = STATE_IDLE; - + } } @@ -429,9 +423,10 @@ void st_cycle_reinitialize() void st_update_plan_block_parameters() { if (pl_block != NULL) { // Ignore if at start of a new block. + prep.flag_partial_block = true; + pl_block->millimeters = prep.millimeters_remaining; pl_block->entry_speed_sqr = prep.current_speed*prep.current_speed; // Update entry speed. pl_block = NULL; // Flag st_prep_segment() to load new velocity profile. - prep.partial_block_flag = true; // Flag st_prep_segment() to indicate block continuation. } } @@ -483,24 +478,22 @@ void st_update_plan_block_parameters() */ void st_prep_buffer() { - if (sys.state == STATE_QUEUED) { return; } // Block until a motion state is issued while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer. - + if (sys.state == STATE_QUEUED) { return; } // Block until a motion state is issued + // Determine if we need to load a new planner block. If so, prepare step data. if (pl_block == NULL) { pl_block = plan_get_current_block(); // Query planner for a queued block if (pl_block == NULL) { return; } // No planner blocks. Exit. - -// SPINDLE_ENABLE_PORT ^= 1<steps[Z_AXIS] = pl_block->steps[Z_AXIS]; st_prep_block->direction_bits = pl_block->direction_bits; st_prep_block->step_event_count = pl_block->step_event_count; - - // Initialize planner block step count, unit distance data, and remainder tracker. - prep.step_per_mm = st_prep_block->step_event_count/pl_block->millimeters; + + // Initialize segment buffer data for generating the segments. prep.step_events_remaining = st_prep_block->step_event_count; - } - - // Compute velocity profile for a feed hold in-process. -// prep.current_speed = sqrt(pl_block->entry_speed_sqr); // !! For a new block, this needs to be updated. -// float inv_2_accel = 0.5/pl_block->acceleration; -// if (sys.state == STATE_HOLD) { -// prep.maximum_speed = prep.current_speed; -// prep.decelerate_after = pl_block->millimeters; -// prep.accelerate_until = pl_block->millimeters; -// prep.ramp_type = DECEL_RAMP; // or FEED_HOLD_RAMP? -// float decelerate_distance = inv_2_accel*pl_block->entry_speed_sqr; -// if (decelerate_distance > pl_block->millimeters) { -// // Keep decelerating through to the end of the block. -// // !! Need to update the next block's entry speed with current speed when loaded. -// prep.exit_speed = sqrt(pl_block->entry_speed_sqr - 2*pl_block->acceleration*pl_block->millimeters); -// } else { -// prep.exit_speed = 0.0; -// ***millimeters = decelerate_distance; // !! Need separate millimeters to track. -// // !! When target mm is reached, don't discard block until it really is at its end. -// // Return state to QUEUED and replan remaining buffer. That's about it. -// } -// } else { - - // Compute the critical velocity profile parameters of the prepped planner block. - prep.current_speed = sqrt(pl_block->entry_speed_sqr); - prep.exit_speed = plan_get_exec_block_exit_speed(); - prep.ramp_type = RAMP_ACCEL; // Initialize as acceleration ramp. - float exit_speed_sqr = prep.exit_speed*prep.exit_speed; - float inv_2_accel = 0.5/pl_block->acceleration; - float intersection_dist = - 0.5*(pl_block->millimeters+inv_2_accel*(pl_block->entry_speed_sqr-exit_speed_sqr)); - if (intersection_dist > 0.0) { - if (intersection_dist < pl_block->millimeters) { // Either trapezoid or triangle types - // NOTE: For acceleration-cruise trapezoid, following calculation will be 0.0. - prep.decelerate_after = inv_2_accel*(pl_block->nominal_speed_sqr-exit_speed_sqr); - if (prep.decelerate_after < intersection_dist) { // Trapezoid type - prep.maximum_speed = sqrt(pl_block->nominal_speed_sqr); - if (pl_block->entry_speed_sqr == pl_block->nominal_speed_sqr) { - // Cruise-deceleration or cruise-only type. - prep.ramp_type = RAMP_CRUISE; - prep.accelerate_until = pl_block->millimeters; - } else { - // Full-trapezoid or acceleration-cruise types - prep.accelerate_until = - pl_block->millimeters-inv_2_accel*(pl_block->nominal_speed_sqr-pl_block->entry_speed_sqr); - } - } else { // Triangle type - prep.accelerate_until = intersection_dist; - prep.decelerate_after = intersection_dist; - prep.maximum_speed = sqrt(2.0*pl_block->acceleration*intersection_dist+exit_speed_sqr); - } - } else { // Deceleration-only type - prep.ramp_type = RAMP_DECEL; - prep.maximum_speed = prep.current_speed; - prep.accelerate_until = pl_block->millimeters; - prep.decelerate_after = pl_block->millimeters; + prep.steps_remaining = st_prep_block->step_event_count; + prep.millimeters_remaining = pl_block->millimeters; + prep.step_per_mm = prep.steps_remaining/prep.millimeters_remaining; + + if (sys.state == STATE_HOLD) { + prep.current_speed = prep.exit_speed; + pl_block->entry_speed_sqr = prep.exit_speed*prep.exit_speed; + } + else { prep.current_speed = sqrt(pl_block->entry_speed_sqr); } + } + + float inv_2_accel = 0.5/pl_block->acceleration; + if (sys.state == STATE_HOLD) { + // Compute velocity profile parameters for a feed hold in-progress. + prep.ramp_type = RAMP_DECEL; + float decel_dist = inv_2_accel*pl_block->entry_speed_sqr; + if (decel_dist < prep.millimeters_remaining) { + prep.exit_speed = 0.0; + prep.steps_remaining = prep.step_per_mm*decel_dist; + prep.millimeters_remaining = decel_dist; + } else { + prep.exit_speed = sqrt(pl_block->entry_speed_sqr-2*pl_block->acceleration*prep.millimeters_remaining); + } + } else { + // Compute velocity profile parameters of the prepped planner block. + prep.ramp_type = RAMP_ACCEL; // Initialize as acceleration ramp. + prep.accelerate_until = prep.millimeters_remaining; + prep.exit_speed = plan_get_exec_block_exit_speed(); + float exit_speed_sqr = prep.exit_speed*prep.exit_speed; + float intersect_distance = + 0.5*(prep.millimeters_remaining+inv_2_accel*(pl_block->entry_speed_sqr-exit_speed_sqr)); + if (intersect_distance > 0.0) { + if (intersect_distance < prep.millimeters_remaining) { // Either trapezoid or triangle types + // NOTE: For acceleration-cruise and cruise-only types, following calculation will be 0.0. + prep.decelerate_after = inv_2_accel*(pl_block->nominal_speed_sqr-exit_speed_sqr); + if (prep.decelerate_after < intersect_distance) { // Trapezoid type + prep.maximum_speed = sqrt(pl_block->nominal_speed_sqr); + if (pl_block->entry_speed_sqr == pl_block->nominal_speed_sqr) { + // Cruise-deceleration or cruise-only type. + prep.ramp_type = RAMP_CRUISE; + } else { + // Full-trapezoid or acceleration-cruise types + prep.accelerate_until -= inv_2_accel*(pl_block->nominal_speed_sqr-pl_block->entry_speed_sqr); + } + } else { // Triangle type + prep.accelerate_until = intersect_distance; + prep.decelerate_after = intersect_distance; + prep.maximum_speed = sqrt(2.0*pl_block->acceleration*intersect_distance+exit_speed_sqr); + } + } else { // Deceleration-only type + prep.ramp_type = RAMP_DECEL; + prep.decelerate_after = prep.millimeters_remaining; + prep.maximum_speed = prep.current_speed; + } + } else { // Acceleration-only type + prep.accelerate_until = 0.0; + prep.decelerate_after = 0.0; + prep.maximum_speed = prep.exit_speed; } - } else { // Acceleration-only type - prep.maximum_speed = prep.exit_speed; - prep.accelerate_until = 0.0; - prep.decelerate_after = 0.0; } } @@ -586,15 +576,15 @@ void st_prep_buffer() /* ----------------------------------------------------------------------------------- Compute the average velocity of this new segment by determining the total distance - traveled over the segment time DT_SEGMENT. The follow code first attempts to create + traveled over the segment time DT_SEGMENT. The following code first attempts to create a full segment based on the current ramp conditions. If the segment time is incomplete - by terminating at a ramp state change, the code will continue to loop through the + when terminating at a ramp state change, the code will continue to loop through the progressing ramp states to fill the remaining segment execution time. However, if an incomplete segment terminates at the end of the planner block, the segment is considered completed despite having a truncated execution time less than DT_SEGMENT. */ float dt = 0.0; - float mm_remaining = pl_block->millimeters; + float mm_remaining = prep.millimeters_remaining; float time_var = DT_SEGMENT; // Time worker variable float mm_var; // mm distance worker variable do { @@ -605,7 +595,7 @@ void st_prep_buffer() if (mm_remaining < prep.accelerate_until) { // End of acceleration ramp. // Acceleration-cruise, acceleration-deceleration ramp junction, or end of block. mm_remaining = prep.accelerate_until; // NOTE: 0.0 at EOB - time_var = 2.0*(pl_block->millimeters-mm_remaining)/(prep.current_speed+prep.maximum_speed); + time_var = 2.0*(prep.millimeters_remaining-mm_remaining)/(prep.current_speed+prep.maximum_speed); if (mm_remaining == prep.decelerate_after) { prep.ramp_type = RAMP_DECEL; } else { prep.ramp_type = RAMP_CRUISE; } prep.current_speed = prep.maximum_speed; @@ -632,11 +622,11 @@ void st_prep_buffer() prep.current_speed -= pl_block->acceleration*time_var; // Check for near-zero speed and prevent divide by zero in rare scenarios. if (prep.current_speed > prep.exit_speed) { mm_remaining -= mm_var; } - else { mm_remaining = 0.0; } // NOTE: Force EOB for now. May change later. + else { mm_remaining = 0.0; } // NOTE: Force EOB for now. May or may not be needed. } else { // End of block. time_var = 2.0*mm_remaining/(prep.current_speed+prep.exit_speed); mm_remaining = 0.0; - // prep.current_speed = prep.exit_speed; + // prep.current_speed = prep.exit_speed; // !! May be needed for feed hold reinitialization. } } dt += time_var; // Add computed ramp time to total segment time. @@ -658,28 +648,45 @@ void st_prep_buffer() time_var = (INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))/dt; // (mult/isr_tic) if (mm_remaining > 0.0) { float steps_remaining = prep.step_per_mm*mm_remaining; - prep_segment->dist_per_tick = ceil( (prep.step_events_remaining-steps_remaining)*time_var ); // (mult*step/isr_tic) + prep_segment->dist_per_tick = ceil( (prep.steps_remaining-steps_remaining)*time_var ); // (mult*step/isr_tic) // Compute number of steps to execute and segment step phase correction. prep_segment->phase_dist = ceil(INV_TIME_MULTIPLIER*(ceil(steps_remaining)-steps_remaining)); - prep_segment->n_step = ceil(prep.step_events_remaining)-ceil(steps_remaining); + prep_segment->n_step = ceil(prep.steps_remaining)-ceil(steps_remaining); // Update step execution variables - pl_block->millimeters = mm_remaining; - prep.step_events_remaining = steps_remaining; + prep.step_events_remaining -= prep_segment->n_step; + prep.millimeters_remaining = mm_remaining; + prep.steps_remaining = steps_remaining; } else { // End of block. // Set to execute the remaining steps and no phase correction upon finishing the block. - prep_segment->dist_per_tick = ceil( prep.step_events_remaining*time_var ); // (mult*step/isr_tic) + prep_segment->dist_per_tick = ceil( prep.steps_remaining*time_var ); // (mult*step/isr_tic) prep_segment->phase_dist = 0; - prep_segment->n_step = ceil(prep.step_events_remaining); + prep_segment->n_step = ceil(prep.steps_remaining); - // The planner block is complete. All steps are set to be executed in the segment buffer. - // TODO: Ignore this for feed holds. Need to recalculate the planner buffer at this time. - pl_block = NULL; - plan_discard_current_block(); + prep.step_events_remaining -= prep_segment->n_step; + if (prep.step_events_remaining > 0) { + sys.state = STATE_QUEUED; + pl_block->entry_speed_sqr = 0.0; + prep.current_speed = 0.0; + prep.steps_remaining = prep.step_events_remaining; + pl_block->millimeters = prep.steps_remaining/prep.step_per_mm; + prep.millimeters_remaining = pl_block->millimeters; + pl_block = NULL; + prep.flag_partial_block = true; + plan_cycle_reinitialize(); + } else { + // The planner block is complete. All steps are set to be executed in the segment buffer. + // TODO: Ignore this for feed holds. Need to recalculate the planner buffer at this time. + pl_block = NULL; + plan_discard_current_block(); + } + } + + // New step segment initialization completed. Increment segment buffer indices. segment_buffer_head = segment_next_head; if ( ++segment_next_head == SEGMENT_BUFFER_SIZE ) { segment_next_head = 0; } @@ -687,6 +694,5 @@ void st_prep_buffer() // int32_t blength = segment_buffer_head - segment_buffer_tail; // if (blength < 0) { blength += SEGMENT_BUFFER_SIZE; } // printInteger(blength); -// SPINDLE_ENABLE_PORT ^= 1< Date: Sat, 7 Dec 2013 08:40:25 -0700 Subject: [PATCH 24/73] Pushed limits active high option. Updated defaults.h. Misc bug fixes. Cleaned up codebase. - Pushed limit switch active high option (i.e. NC switches). - Updated defaults.h to be in-line with the new settings. - Refactored feed hold handling and step segment buffer to be more generalized in effort to make adding feedrate overrides easier in the future. Also made it a little more clean. - Fixed G18 plane select issue. Now ZX-plane, rather than XZ-plane, per right hand rule. - Cleaned some of the system settings by more accurately renaming some of the variables and removing old obsolete ones. - Declared serial.c rx_buffer_tail to be volatile. No effect, since avr-gcc automatically does this during compilation. Helps with porting when using other compilers. - Updated version number to v0.9b. - Updates to README.md --- README.md | 10 +- archive/planner_dist.c | 669 --------------------------- archive/planner_time_archive.c | 459 ------------------ archive/planner_v0_9.c | 476 ------------------- archive/planner_v0_9.h | 83 ---- archive/stepper_dist.c | 706 ---------------------------- archive/stepper_old.c | 746 ------------------------------ archive/stepper_time.c | 775 ------------------------------- archive/stepper_time_archive.c | 823 --------------------------------- archive/stepper_v0_9.c | 387 ---------------- config.h | 6 + defaults.h | 116 +++-- gcode.c | 2 +- limits.c | 16 +- planner.c | 14 +- planner.h | 29 +- report.c | 8 +- serial.c | 16 +- settings.c | 22 +- settings.h | 18 +- stepper.c | 152 +++--- stepper_test.c | 788 ------------------------------- 22 files changed, 220 insertions(+), 6101 deletions(-) delete mode 100644 archive/planner_dist.c delete mode 100644 archive/planner_time_archive.c delete mode 100644 archive/planner_v0_9.c delete mode 100644 archive/planner_v0_9.h delete mode 100644 archive/stepper_dist.c delete mode 100644 archive/stepper_old.c delete mode 100644 archive/stepper_time.c delete mode 100644 archive/stepper_time_archive.c delete mode 100644 archive/stepper_v0_9.c delete mode 100644 stepper_test.c diff --git a/README.md b/README.md index 0ed3006..e29a19e 100644 --- a/README.md +++ b/README.md @@ -5,22 +5,24 @@ This branch serves only as a developmental platform for working on new ideas tha ------------ -Grbl is a no-compromise, high performance, low cost alternative to parallel-port-based motion control for CNC milling. It will run on a vanilla Arduino (Duemillanove/Uno) as long as it sports an Atmega 328. +Grbl is a no-compromise, high performance, low cost alternative to parallel-port-based motion control for CNC milling. It will run on a vanilla Arduino (Duemillanove/Uno) as long as it sports an Atmega 328. (Other AVR CPUs are unofficially supported as well.) -The controller is written in highly optimized C utilizing every clever feature of the AVR-chips to achieve precise timing and asynchronous operation. It is able to m aintain more than 30kHz of stable, jitter free control pulses. +The controller is written in highly optimized C utilizing every clever feature of the AVR-chips to achieve precise timing and asynchronous operation. It is able to maintain up to 30kHz of stable, jitter free control pulses. -It accepts standards-compliant G-code and has been tested with the output of several CAM tools with no problems. Arcs, circles and helical motion are fully supported, as well as, other basic functional g-code commands. Functions and variables are not currently supported, but may be included in future releases in a form of a pre-processor. +It accepts standards-compliant G-code and has been tested with the output of several CAM tools with no problems. Arcs, circles and helical motion are fully supported, as well as, other basic functional g-code commands. Although canned cycles, functions, and variables are not currently supported (may in the future), GUIs can be built or modified easily to translate to straight g-code for Grbl. Grbl includes full acceleration management with look ahead. That means the controller will look up to 18 motions into the future and plan its velocities ahead to deliver smooth acceleration and jerk-free cornering. ##Changelog for v0.9 from v0.8 - **ALPHA status: Under heavy development.** - New stepper algorithm: Based on an inverse time algorithm, but modified to ensure steps are executed exactly. This algorithm performs a constant timer tick and has a hard limit of 30kHz maximum step frequency. It is also highly tuneable and should be very easy to port to other microcontroller architectures. Overall, a much better, smoother stepper algorithm with the capability of very high speeds. - - Planner optimizations: Multiple changes to increase planner execution speed and removed redundant variables. + - Planner optimizations: Planning computations improved four-fold or more. Changes include streaming optimizations by ignoring already optimized blocks and removing redundant variables and computations and offloading them to the stepper algorithm on an ad-hoc basis. + - Planner stability: Previous Grbl planners have all had a corruption issue in rare circumstances that becomes particularly problematic at high step frequencies and when jogging. The new planner is robust and incorruptible, meaning that we can fearlessly drive Grbl to it's highest limits. Combined with the new stepper algorithm and planner optimizations, this means 5x to 10x performance increases in our testing! This is all achieved through the introduction of an intermediary step segment buffer that "checks-out" steps from the planner buffer in real-time. - Acceleration independence: Each axes may be defined with different acceleration parameters and Grbl will automagically calculate the maximum acceleration through a path depending on the direction traveled. This is very useful for machine that have very different axes properties, like the ShapeOko z-axis. - Maximum velocity independence: As with acceleration, the maximum velocity of individual axes may be defined. All seek/rapids motions will move at these maximum rates, but never exceed any one axes. So, when two or more axes move, the limiting axis will move at its maximum rate, while the other axes are scaled down. - Significantly improved arc performance: Arcs are now defined in terms of chordal tolerance, rather than segment length. Chordal tolerance will automatically scale all arc line segments depending on arc radius, such that the error does not exceed the tolerance value (default: 0.005 mm.) So, for larger radii arcs, Grbl can move faster through them, because the segments are always longer and the planner has more distance to plan with. - Soft limits: Checks if any motion command exceeds workspace limits. Alarms out when found. Another safety feature, but, unlike hard limits, position does not get lost, as it forces a feed hold before erroring out. + - Pin mapping: In an effort for Grbl to be compatible with other AVR architectures, such as the 1280 or 2560, a new pin_map.h configuration file has been created to allow Grbl to be compiled for them. This is currently user supported, so your mileage may vary. If you run across a bug, please let us know or better send us a fix! Thanks in advance! - New Grbl SIMULATOR by @jgeisler: A completely independent wrapper of the Grbl main source code that may be compiled as an executable on a computer. No Arduino required. Simply simulates the responses of Grbl as if it was on an Arduino. May be used for many things: checking out how Grbl works, pre-process moves for GUI graphics, debugging of new features, etc. Much left to do, but potentially very powerful, as the dummy AVR variables can be written to output anything you need. - Homing routine updated: Sets workspace volume in all negative space regardless of limit switch position. Common on pro CNCs. Also reduces soft limits CPU overhead. - Feedrate overrides: In the works, but planner has begun to be re-factored for this feature. diff --git a/archive/planner_dist.c b/archive/planner_dist.c deleted file mode 100644 index 0afa018..0000000 --- a/archive/planner_dist.c +++ /dev/null @@ -1,669 +0,0 @@ -/* - planner.c - buffers movement commands and manages the acceleration profile plan - Part of Grbl - - Copyright (c) 2011-2013 Sungeun K. Jeon - Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2011 Jens Geisler - - 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 - (at your option) any later version. - - Grbl is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with Grbl. If not, see . -*/ - -/* The ring buffer implementation gleaned from the wiring_serial library by David A. Mellis. */ - -#include -#include -#include "planner.h" -#include "nuts_bolts.h" -#include "stepper.h" -#include "settings.h" -#include "config.h" -#include "protocol.h" - -#define SOME_LARGE_VALUE 1.0E+38 // Used by rapids and acceleration maximization calculations. Just needs - // to be larger than any feasible (mm/min)^2 or mm/sec^2 value. - -static plan_block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instructions -static volatile uint8_t block_buffer_tail; // Index of the block to process now -static uint8_t block_buffer_head; // Index of the next block to be pushed -static uint8_t next_buffer_head; // Index of the next buffer head -static uint8_t block_buffer_planned; // Index of the optimally planned block - -// Define planner variables -typedef struct { - int32_t position[N_AXIS]; // The planner position of the tool in absolute steps. Kept separate - // from g-code position for movements requiring multiple line motions, - // i.e. arcs, canned cycles, and backlash compensation. - float previous_unit_vec[N_AXIS]; // Unit vector of previous path line segment - float previous_nominal_speed_sqr; // Nominal speed of previous path line segment -} planner_t; -static planner_t pl; - - -// Returns the index of the next block in the ring buffer. Also called by stepper segment buffer. -// NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication. -uint8_t plan_next_block_index(uint8_t block_index) -{ - block_index++; - if (block_index == BLOCK_BUFFER_SIZE) { block_index = 0; } - return(block_index); -} - - -// Returns the index of the previous block in the ring buffer -static uint8_t plan_prev_block_index(uint8_t block_index) -{ - if (block_index == 0) { block_index = BLOCK_BUFFER_SIZE; } - block_index--; - return(block_index); -} - - -// Update the entry speed and millimeters remaining to execute for a partially completed block. Called only -// when the planner knows it will be changing the conditions of this block. -// TODO: Set up to be called from planner calculations. Need supporting code framework still, i.e. checking -// and executing this only when necessary, combine with the block_buffer_safe pointer. -// TODO: This is very similar to the planner reinitialize after a feed hold. Could make this do double duty. -void plan_update_partial_block(uint8_t block_index, float exit_speed_sqr) -{ -// TODO: Need to make a condition to check if we need make these calculations. We don't if nothing has -// been executed or placed into segment buffer. This happens with the first block upon startup or if -// the segment buffer is exactly in between two blocks. Just check if the step_events_remaining is equal -// the total step_event_count in the block. If so, we don't have to do anything. - - // !!! block index is the same as block_buffer_safe. - // See if we can reduce this down to just requesting the millimeters remaining.. - uint8_t is_decelerating; - float millimeters_remaining = 0.0; - st_fetch_partial_block_parameters(block_index, &millimeters_remaining, &is_decelerating); - - if (millimeters_remaining != 0.0) { - // Point to current block partially executed by stepper algorithm - plan_block_t *partial_block = plan_get_block_by_index(block_index); - - // Compute the midway speed of the partially completely block at the end of the segment buffer. - if (is_decelerating) { // Block is decelerating - partial_block->entry_speed_sqr = exit_speed_sqr - 2*partial_block->acceleration*millimeters_remaining; - } else { // Block is accelerating or cruising - partial_block->entry_speed_sqr += 2*partial_block->acceleration*(partial_block->millimeters-millimeters_remaining); - partial_block->entry_speed_sqr = min(partial_block->entry_speed_sqr, partial_block->nominal_speed_sqr); - } - - // Update only the relevant planner block information so the planner can plan correctly. - partial_block->millimeters = millimeters_remaining; - partial_block->max_entry_speed_sqr = partial_block->entry_speed_sqr; // Not sure if this needs to be updated. - } -} - - -/* PLANNER SPEED DEFINITION - +--------+ <- current->nominal_speed - / \ - current->entry_speed -> + \ - | + <- next->entry_speed (aka exit speed) - +-------------+ - time --> - - Recalculates the motion plan according to the following basic guidelines: - - 1. Go over every feasible block sequentially in reverse order and calculate the junction speeds - (i.e. current->entry_speed) such that: - a. No junction speed exceeds the pre-computed maximum junction speed limit or nominal speeds of - neighboring blocks. - b. A block entry speed cannot exceed one reverse-computed from its exit speed (next->entry_speed) - with a maximum allowable deceleration over the block travel distance. - c. The last (or newest appended) block is planned from a complete stop (an exit speed of zero). - 2. Go over every block in chronological (forward) order and dial down junction speed values if - a. The exit speed exceeds the one forward-computed from its entry speed with the maximum allowable - acceleration over the block travel distance. - - When these stages are complete, the planner will have maximized the velocity profiles throughout the all - of the planner blocks, where every block is operating at its maximum allowable acceleration limits. In - other words, for all of the blocks in the planner, the plan is optimal and no further speed improvements - are possible. If a new block is added to the buffer, the plan is recomputed according to the said - guidelines for a new optimal plan. - - To increase computational efficiency of these guidelines, a set of planner block pointers have been - created to indicate stop-compute points for when the planner guidelines cannot logically make any further - changes or improvements to the plan when in normal operation and new blocks are streamed and added to the - planner buffer. For example, if a subset of sequential blocks in the planner have been planned and are - bracketed by junction velocities at their maximums (or by the first planner block as well), no new block - added to the planner buffer will alter the velocity profiles within them. So we no longer have to compute - them. Or, if a set of sequential blocks from the first block in the planner (or a optimal stop-compute - point) are all accelerating, they are all optimal and can not be altered by a new block added to the - planner buffer, as this will only further increase the plan speed to chronological blocks until a maximum - junction velocity is reached. However, if the operational conditions of the plan changes from infrequently - used feed holds or feedrate overrides, the stop-compute pointers will be reset and the entire plan is - recomputed as stated in the general guidelines. - - Planner buffer index mapping: - - block_buffer_tail: Points to the beginning of the planner buffer. First to be executed or being executed. - - block_buffer_head: Points to the buffer block after the last block in the buffer. Used to indicate whether - the buffer is full or empty. As described for standard ring buffers, this block is always empty. - - next_buffer_head: Points to next planner buffer block after the buffer head block. When equal to the - buffer tail, this indicates the buffer is full. - - block_buffer_safe: Points to the first sequential planner block for which it is safe to recompute, which - is defined to be where the stepper's step segment buffer ends. This may or may not be the buffer tail, - since the step segment buffer queues steps which may have not finished executing and could span a few - blocks, if the block moves are very short. - - block_buffer_planned: Points to the first buffer block after the last optimally planned block for normal - streaming operating conditions. Use for planning optimizations by avoiding recomputing parts of the - planner buffer that don't change with the addition of a new block, as describe above. - - NOTE: All planner computations are performed in floating point to minimize numerical round-off errors. - When a planner block is executed, the floating point values are converted to fast integers by the stepper - algorithm segment buffer. See the stepper module for details. - - NOTE: Since the planner only computes on what's in the planner buffer, some motions with lots of short - line segments, like G2/3 arcs or complex curves, may seem to move slow. This is because there simply isn't - enough combined distance traveled in the entire buffer to accelerate up to the nominal speed and then - decelerate to a complete stop at the end of the buffer, as stated by the guidelines. If this happens and - becomes an annoyance, there are a few simple solutions: (1) Maximize the machine acceleration. The planner - will be able to compute higher velocity profiles within the same combined distance. (2) Maximize line - segment(s) distance per block to a desired tolerance. The more combined distance the planner has to use, - the faster it can go. (3) Maximize the planner buffer size. This also will increase the combined distance - for the planner to compute over. It also increases the number of computations the planner has to perform - to compute an optimal plan, so select carefully. The Arduino 328p memory is already maxed out, but future - ARM versions should have enough memory and speed for look-ahead blocks numbering up to a hundred or more. - -*/ -static void planner_recalculate() -{ - - // Initialize block index to the last block in the planner buffer. - uint8_t block_index = plan_prev_block_index(block_buffer_head); - - // Query stepper module for safe planner block index to recalculate to, which corresponds to the end - // of the step segment buffer. - uint8_t block_buffer_safe = st_get_prep_block_index(); - - // TODO: Make sure that we don't have to check for the block_buffer_tail condition, if the stepper module - // returns a NULL pointer or something. This could happen when the segment buffer is empty. Although, - // this call won't return a NULL, only an index.. I have to make sure that this index is synced with the - // planner at all times. - - // Recompute plan only when there is more than one planner block in the buffer. Can't do anything with one. - // NOTE: block_buffer_safe can be the last planner block if the segment buffer has completely queued up the - // remainder of the planner buffer. In this case, a new planner block will be treated as a single block. - if (block_index == block_buffer_safe) { // Also catches (head-1) = tail - - // Just set block_buffer_planned pointer. - block_buffer_planned = block_index; - - // TODO: Feedrate override of one block needs to update the partial block with an exit speed of zero. For - // a single added block and recalculate after a feed hold, we don't need to compute this, since we already - // know that the velocity starts and ends at zero. With an override, we can be traveling at some midblock - // rate, and we have to calculate the new velocity profile from it. - // plan_update_partial_block(block_index,0.0); - - } else { - - // TODO: If the nominal speeds change during a feedrate override, we need to recompute the max entry speeds for - // all junctions before proceeding. - - // Initialize planner buffer pointers and indexing. - plan_block_t *current = &block_buffer[block_index]; - - // Calculate maximum entry speed for last block in buffer, where the exit speed is always zero. - current->entry_speed_sqr = min( current->max_entry_speed_sqr, 2*current->acceleration*current->millimeters); - - // Reverse Pass: Coarsely maximize all possible deceleration curves back-planning from the last - // block in buffer. Cease planning when: (1) the last optimal planned pointer is reached. - // (2) the safe block pointer is reached, whereby the planned pointer is updated. - // NOTE: Forward pass will later refine and correct the reverse pass to create an optimal plan. - // NOTE: If the safe block is encountered before the planned block pointer, we know the safe block - // will be recomputed within the plan. So, we need to update it if it is partially completed. - float entry_speed_sqr; - plan_block_t *next; - block_index = plan_prev_block_index(block_index); - - if (block_index == block_buffer_safe) { // !! OR plan pointer? Yes I think so. - - // Only two plannable blocks in buffer. Compute previous block based on - // !!! May only work if a new block is being added. Not for an override. The exit speed isn't zero. - // !!! Need to make the current entry speed calculation after this. - plan_update_partial_block(block_index, 0.0); - block_buffer_planned = block_index; - - } else { - - // Three or more plan-able - while (block_index != block_buffer_planned) { - - next = current; - current = &block_buffer[block_index]; - - // Increment block index early to check if the safe block is before the current block. If encountered, - // this is an exit condition as we can't go further than this block in the reverse pass. - block_index = plan_prev_block_index(block_index); - if (block_index == block_buffer_safe) { - // Check if the safe block is partially completed. If so, update it before its exit speed - // (=current->entry speed) is over-written. - // TODO: The update breaks with feedrate overrides, because the replanning process no longer has - // the previous nominal speed to update this block with. There will need to be something along the - // lines of a nominal speed change check and send the correct value to this function. - plan_update_partial_block(block_index,current->entry_speed_sqr); - - // Set planned pointer at safe block and for loop exit after following computation is done. - block_buffer_planned = block_index; - } - - // Compute maximum entry speed decelerating over the current block from its exit speed. - if (current->entry_speed_sqr != current->max_entry_speed_sqr) { - entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters; - if (entry_speed_sqr < current->max_entry_speed_sqr) { - current->entry_speed_sqr = entry_speed_sqr; - } else { - current->entry_speed_sqr = current->max_entry_speed_sqr; - } - } - } - - } - - // Forward Pass: Forward plan the acceleration curve from the planned pointer onward. - // Also scans for optimal plan breakpoints and appropriately updates the planned pointer. - next = &block_buffer[block_buffer_planned]; // Begin at buffer planned pointer - block_index = plan_next_block_index(block_buffer_planned); - while (block_index != block_buffer_head) { - current = next; - next = &block_buffer[block_index]; - - // Any acceleration detected in the forward pass automatically moves the optimal planned - // pointer forward, since everything before this is all optimal. In other words, nothing - // can improve the plan from the buffer tail to the planned pointer by logic. - // TODO: Need to check if the planned flag logic is correct for all scenarios. It may not - // be for certain conditions. However, if the block reaches nominal speed, it can be a valid - // breakpoint substitute. - if (current->entry_speed_sqr < next->entry_speed_sqr) { - entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters; - // If true, current block is full-acceleration and we can move the planned pointer forward. - if (entry_speed_sqr < next->entry_speed_sqr) { - next->entry_speed_sqr = entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass sets this. - block_buffer_planned = block_index; // Set optimal plan pointer. - } - } - - // Any block set at its maximum entry speed also creates an optimal plan up to this - // point in the buffer. When the plan is bracketed by either the beginning of the - // buffer and a maximum entry speed or two maximum entry speeds, every block in between - // cannot logically be further improved. Hence, we don't have to recompute them anymore. - if (next->entry_speed_sqr == next->max_entry_speed_sqr) { - block_buffer_planned = block_index; // Set optimal plan pointer - } - - block_index = plan_next_block_index( block_index ); - } - - } - -} - - -void plan_reset_buffer() -{ - block_buffer_planned = block_buffer_tail; -} - - -void plan_init() -{ - block_buffer_tail = 0; - block_buffer_head = 0; // Empty = tail - next_buffer_head = 1; // plan_next_block_index(block_buffer_head) - plan_reset_buffer(); - memset(&pl, 0, sizeof(pl)); // Clear planner struct -} - - -void plan_discard_current_block() -{ - if (block_buffer_head != block_buffer_tail) { // Discard non-empty buffer. - block_buffer_tail = plan_next_block_index( block_buffer_tail ); - } -} - - -plan_block_t *plan_get_current_block() -{ - if (block_buffer_head == block_buffer_tail) { // Buffer empty - plan_reset_buffer(); - return(NULL); - } - return(&block_buffer[block_buffer_tail]); -} - - -plan_block_t *plan_get_block_by_index(uint8_t block_index) -{ - if (block_buffer_head == block_index) { return(NULL); } - return(&block_buffer[block_index]); -} - - -// Returns the availability status of the block ring buffer. True, if full. -uint8_t plan_check_full_buffer() -{ - if (block_buffer_tail == next_buffer_head) { return(true); } - return(false); -} - - -// Block until all buffered steps are executed or in a cycle state. Works with feed hold -// during a synchronize call, if it should happen. Also, waits for clean cycle end. -void plan_synchronize() -{ - while (plan_get_current_block() || sys.state == STATE_CYCLE) { - protocol_execute_runtime(); // Check and execute run-time commands - if (sys.abort) { return; } // Check for system abort - } -} - - -/* Add a new linear movement to the buffer. target[N_AXIS] is the signed, absolute target position - in millimeters. 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. - All position data passed to the planner must be in terms of machine position to keep the planner - independent of any coordinate system changes and offsets, which are handled by the g-code parser. - NOTE: Assumes buffer is available. Buffer checks are handled at a higher level by motion_control. - In other words, the buffer head is never equal to the buffer tail. Also the feed rate input value - is used in three ways: as a normal feed rate if invert_feed_rate is false, as inverse time if - invert_feed_rate is true, or as seek/rapids rate if the feed_rate value is negative (and - invert_feed_rate always false). */ -void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) -{ - // Prepare and initialize new block - plan_block_t *block = &block_buffer[block_buffer_head]; - block->step_event_count = 0; - block->millimeters = 0; - block->direction_bits = 0; - block->acceleration = SOME_LARGE_VALUE; // Scaled down to maximum acceleration later - - // Compute and store initial move distance data. - // TODO: After this for-loop, we don't touch the stepper algorithm data. Might be a good idea - // to try to keep these types of things completely separate from the planner for portability. - int32_t target_steps[N_AXIS]; - float unit_vec[N_AXIS], delta_mm; - uint8_t idx; - for (idx=0; idxsteps[idx] = labs(target_steps[idx]-pl.position[idx]); - block->step_event_count = max(block->step_event_count, block->steps[idx]); - - // Compute individual axes distance for move and prep unit vector calculations. - // NOTE: Computes true distance from converted step values. - delta_mm = (target_steps[idx] - pl.position[idx])/settings.steps_per_mm[idx]; - unit_vec[idx] = delta_mm; // Store unit vector numerator. Denominator computed later. - - // Set direction bits. Bit enabled always means direction is negative. - if (delta_mm < 0 ) { block->direction_bits |= get_direction_mask(idx); } - - // Incrementally compute total move distance by Euclidean norm. First add square of each term. - block->millimeters += delta_mm*delta_mm; - } - block->millimeters = sqrt(block->millimeters); // Complete millimeters calculation with sqrt() - - // Bail if this is a zero-length block. Highly unlikely to occur. - if (block->step_event_count == 0) { return; } - - // Adjust feed_rate value to mm/min depending on type of rate input (normal, inverse time, or rapids) - // TODO: Need to distinguish a rapids vs feed move for overrides. Some flag of some sort. - if (feed_rate < 0) { feed_rate = SOME_LARGE_VALUE; } // Scaled down to absolute max/rapids rate later - else if (invert_feed_rate) { feed_rate = block->millimeters/feed_rate; } - - // Calculate the unit vector of the line move and the block maximum feed rate and acceleration scaled - // down such that no individual axes maximum values are exceeded with respect to the line direction. - // NOTE: This calculation assumes all axes are orthogonal (Cartesian) and works with ABC-axes, - // if they are also orthogonal/independent. Operates on the absolute value of the unit vector. - float inverse_unit_vec_value; - float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple float divides - float junction_cos_theta = 0; - for (idx=0; idxacceleration = min(block->acceleration,settings.acceleration[idx]*inverse_unit_vec_value); - - // Incrementally compute cosine of angle between previous and current path. Cos(theta) of the junction - // between the current move and the previous move is simply the dot product of the two unit vectors, - // where prev_unit_vec is negative. Used later to compute maximum junction speed. - junction_cos_theta -= pl.previous_unit_vec[idx] * unit_vec[idx]; - } - } - - - // TODO: Need to check this method handling zero junction speeds when starting from rest. - if (block_buffer_head == block_buffer_tail) { - - // Initialize block entry speed as zero. Assume it will be starting from rest. Planner will correct this later. - block->entry_speed_sqr = 0.0; - block->max_junction_speed_sqr = 0.0; // Starting from rest. Enforce start from zero velocity. - - } else { - /* - Compute maximum allowable entry speed at junction by centripetal acceleration approximation. - 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 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. This approach does not actually deviate - from path, but used as a robust way to compute cornering speeds, as it takes into account the - nonlinearities of both the junction angle and junction velocity. - - NOTE: If the junction deviation value is finite, Grbl executes the motions in an exact path - mode (G61). If the junction deviation value is zero, Grbl will execute the motion in an exact - stop mode (G61.1) manner. In the future, if continuous mode (G64) is desired, the math here - is exactly the same. Instead of motioning all the way to junction point, the machine will - just follow the arc circle defined here. The Arduino doesn't have the CPU cycles to perform - a continuous mode path, but ARM-based microcontrollers most certainly do. - - NOTE: The max junction speed is a fixed value, since machine acceleration limits cannot be - changed dynamically during operation nor can the line move geometry. This must be kept in - memory in the event of a feedrate override changing the nominal speeds of blocks, which can - change the overall maximum entry speed conditions of all blocks. - */ - // NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta). - float sin_theta_d2 = sqrt(0.5*(1.0-junction_cos_theta)); // Trig half angle identity. Always positive. - - // TODO: Acceleration used in calculation needs to be limited by the minimum of the two junctions. - block->max_junction_speed_sqr = max( MINIMUM_JUNCTION_SPEED*MINIMUM_JUNCTION_SPEED, - (block->acceleration * settings.junction_deviation * sin_theta_d2)/(1.0-sin_theta_d2) ); - } - - // Store block nominal speed - block->nominal_speed_sqr = feed_rate*feed_rate; // (mm/min). Always > 0 - - // Compute the junction maximum entry based on the minimum of the junction speed and neighboring nominal speeds. - block->max_entry_speed_sqr = min(block->max_junction_speed_sqr, - min(block->nominal_speed_sqr,pl.previous_nominal_speed_sqr)); - - // Update previous path unit_vector and nominal speed (squared) - memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // pl.previous_unit_vec[] = unit_vec[] - pl.previous_nominal_speed_sqr = block->nominal_speed_sqr; - - // Update planner position - memcpy(pl.position, target_steps, sizeof(target_steps)); // pl.position[] = target_steps[] - - // New block is all set. Update buffer head and next buffer head indices. - block_buffer_head = next_buffer_head; - next_buffer_head = plan_next_block_index(block_buffer_head); - - // Finish up by recalculating the plan with the new block. - planner_recalculate(); - -// int32_t blength = block_buffer_head - block_buffer_tail; -// if (blength < 0) { blength += BLOCK_BUFFER_SIZE; } -// printInteger(blength); - - -} - - -// Reset the planner position vectors. Called by the system abort/initialization routine. -void plan_sync_position() -{ - uint8_t idx; - for (idx=0; idx + - +--------+ <- nominal_speed /|\ - / \ / | \ - entry_speed -> + \ / | + <- next->entry_speed - | + <- next->entry_speed / | | - +-------------+ entry_speed -> +----+--+ - time --> ^ ^ ^ ^ - | | | | - decelerate distance decelerate distance - - Calculates the type of velocity profile for a given planner block and provides the deceleration - distance for the stepper algorithm to use to accurately trace the profile exactly. The planner - computes the entry and exit speeds of each block, but does not bother to determine the details of - the velocity profiles within them, as they aren't needed for computing an optimal plan. When the - stepper algorithm begins to execute a block, the block velocity profiles are computed ad hoc. - - Each block velocity profiles can be described as either a trapezoidal or a triangular shape. The - trapezoid occurs when the block reaches the nominal speed of the block and cruises for a period of - time. A triangle occurs when the nominal speed is not reached within the block. Both of these - velocity profiles may also be truncated on either end with no acceleration or deceleration ramps, - as they can be influenced by the conditions of neighboring blocks, where the acceleration ramps - are defined by constant acceleration equal to the maximum allowable acceleration of a block. - - Since the stepper algorithm already assumes to begin executing a planner block by accelerating - from the planner entry speed and cruise if the nominal speed is reached, we only need to know - when to begin deceleration to the end of the block. Hence, only the distance from the end of the - block to begin a deceleration ramp is computed for the stepper algorithm when requested. -*/ -float plan_calculate_velocity_profile(uint8_t block_index) -{ - plan_block_t *current_block = &block_buffer[block_index]; - - // Determine current block exit speed - float exit_speed_sqr = 0.0; // Initialize for end of planner buffer. Zero speed. - plan_block_t *next_block = plan_get_block_by_index(plan_next_block_index(block_index)); - if (next_block != NULL) { exit_speed_sqr = next_block->entry_speed_sqr; } // Exit speed is the entry speed of next buffer block - - // First determine intersection distance (in steps) from the exit point for a triangular profile. - // Computes: d_intersect = distance/2 + (v_entry^2-v_exit^2)/(4*acceleration) - float intersect_distance = 0.5*( current_block->millimeters + (current_block->entry_speed_sqr-exit_speed_sqr)/(2*current_block->acceleration) ); - - // Check if this is a pure acceleration block by a intersection distance less than zero. Also - // prevents signed and unsigned integer conversion errors. - if (intersect_distance > 0 ) { - float decelerate_distance; - // Determine deceleration distance (in steps) from nominal speed to exit speed for a trapezoidal profile. - // Value is never negative. Nominal speed is always greater than or equal to the exit speed. - // Computes: d_decelerate = (v_nominal^2 - v_exit^2)/(2*acceleration) - decelerate_distance = (current_block->nominal_speed_sqr - exit_speed_sqr)/(2*current_block->acceleration); - - // The lesser of the two triangle and trapezoid distances always defines the velocity profile. - if (decelerate_distance > intersect_distance) { decelerate_distance = intersect_distance; } - - // Finally, check if this is a pure deceleration block. - if (decelerate_distance > current_block->millimeters) { return(0.0); } - else { return( (current_block->millimeters-decelerate_distance) ); } - } - return( current_block->millimeters ); // No deceleration in velocity profile. -} - - -// Re-initialize buffer plan with a partially completed block, assumed to exist at the buffer tail. -// Called after a steppers have come to a complete stop for a feed hold and the cycle is stopped. -void plan_cycle_reinitialize(int32_t step_events_remaining) -{ - plan_block_t *block = &block_buffer[block_buffer_tail]; // Point to partially completed block - - // Only remaining millimeters and step_event_count need to be updated for planner recalculate. - // Other variables (step_x, step_y, step_z, rate_delta, etc.) all need to remain the same to - // ensure the original planned motion is resumed exactly. - block->millimeters = (block->millimeters*step_events_remaining)/block->step_event_count; - block->step_event_count = step_events_remaining; - - // Re-plan from a complete stop. Reset planner entry speeds and buffer planned pointer. - block->entry_speed_sqr = 0.0; - block->max_entry_speed_sqr = 0.0; - block_buffer_planned = block_buffer_tail; - planner_recalculate(); -} - - -/* -TODO: - When a feed hold or feedrate override is reduced, the velocity profile must execute a - deceleration over the existing plan. By logic, since the plan already decelerates to zero - at the end of the buffer, any replanned deceleration mid-way will never exceed this. It - will only asymptotically approach this in the worst case scenario. - - - For a feed hold, we simply need to plan and compute the stopping point within a block - when velocity decelerates to zero. We then can recompute the plan with the already - existing partial block planning code and set the system to a QUEUED state. - - When a feed hold is initiated, the main program should be able to continue doing what - it has been, i.e. arcs, parsing, but needs to be able to reinitialize the plan after - it has come to a stop. - - - For a feed rate override (reduce-only), we need to enforce a deceleration until we - intersect the reduced nominal speed of a block after it's been planned with the new - overrides and the newly planned block is accelerating or cruising only. If the new plan - block is decelerating at the intersection point, we keep decelerating until we find a - valid intersection point. Once we find this point, we can then resume onto the new plan, - but we may need to adjust the deceleration point in the intersection block since the - feedrate override could have intersected at an acceleration ramp. This would change the - acceleration ramp to a cruising, so the deceleration point will have changed, but the - plan will have not. It should still be valid for the rest of the buffer. Coding this - can get complicated, but it should be doable. One issue could be is in how to handle - scenarios when a user issues several feedrate overrides and inundates this code. Does - this method still work and is robust enough to compute all of this on the fly? This is - the critical question. However, we could block user input until the planner has time to - catch to solve this as well. - - - When the feed rate override increases, we don't have to do anything special. We just - replan the entire buffer with the new nominal speeds and adjust the maximum junction - speeds accordingly. - -void plan_compute_deceleration() { - -} - - -void plan_recompute_max_junction_velocity() { - // Assumes the nominal_speed_sqr values have been updated. May need to just multiply - // override values here. - // PROBLEM: Axes-limiting velocities get screwed up. May need to store an int8 value for the - // max override value possible for each block when the line is added. So the nominal_speed - // is computed with that ceiling, but still retained if the rates change again. - uint8_t block_index = block_buffer_tail; - plan_block_t *block = &block_buffer[block_index]; - pl.previous_nominal_speed_sqr = block->nominal_speed_sqr; - block_index = plan_next_block_index(block_index); - while (block_index != block_buffer_head) { - block = &block_buffer[block_index]; - block->max_entry_speed_sqr = min(block->max_junction_speed_sqr, - min(block->nominal_speed_sqr,pl.previous_nominal_speed_sqr)); - pl.previous_nominal_speed_sqr = block->nominal_speed_sqr; - block_index = plan_next_block_index(block_index); - } -} - -*/ diff --git a/archive/planner_time_archive.c b/archive/planner_time_archive.c deleted file mode 100644 index 9f584df..0000000 --- a/archive/planner_time_archive.c +++ /dev/null @@ -1,459 +0,0 @@ -/* - planner.c - buffers movement commands and manages the acceleration profile plan - Part of Grbl - - Copyright (c) 2011-2013 Sungeun K. Jeon - Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2011 Jens Geisler - - 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 - (at your option) any later version. - - Grbl is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with Grbl. If not, see . -*/ - -/* The ring buffer implementation gleaned from the wiring_serial library by David A. Mellis. */ - -#include -#include -#include "planner.h" -#include "nuts_bolts.h" -#include "stepper.h" -#include "settings.h" -#include "config.h" -#include "protocol.h" - -#define SOME_LARGE_VALUE 1.0E+38 // Used by rapids and acceleration maximization calculations. Just needs - // to be larger than any feasible (mm/min)^2 or mm/sec^2 value. - -static plan_block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instructions -static uint8_t block_buffer_tail; // Index of the block to process now -static uint8_t block_buffer_head; // Index of the next block to be pushed -static uint8_t next_buffer_head; // Index of the next buffer head -static uint8_t block_buffer_planned; // Index of the optimally planned block - -// Define planner variables -typedef struct { - int32_t position[N_AXIS]; // The planner position of the tool in absolute steps. Kept separate - // from g-code position for movements requiring multiple line motions, - // i.e. arcs, canned cycles, and backlash compensation. - float previous_unit_vec[N_AXIS]; // Unit vector of previous path line segment - float previous_nominal_speed_sqr; // Nominal speed of previous path line segment -} planner_t; -static planner_t pl; - - -// Returns the index of the next block in the ring buffer. Also called by stepper segment buffer. -uint8_t plan_next_block_index(uint8_t block_index) -{ - block_index++; - if (block_index == BLOCK_BUFFER_SIZE) { block_index = 0; } - return(block_index); -} - - -// Returns the index of the previous block in the ring buffer -static uint8_t plan_prev_block_index(uint8_t block_index) -{ - if (block_index == 0) { block_index = BLOCK_BUFFER_SIZE; } - block_index--; - return(block_index); -} - - -/* PLANNER SPEED DEFINITION - +--------+ <- current->nominal_speed - / \ - current->entry_speed -> + \ - | + <- next->entry_speed (aka exit speed) - +-------------+ - time --> - - Recalculates the motion plan according to the following basic guidelines: - - 1. Go over every feasible block sequentially in reverse order and calculate the junction speeds - (i.e. current->entry_speed) such that: - a. No junction speed exceeds the pre-computed maximum junction speed limit or nominal speeds of - neighboring blocks. - b. A block entry speed cannot exceed one reverse-computed from its exit speed (next->entry_speed) - with a maximum allowable deceleration over the block travel distance. - c. The last (or newest appended) block is planned from a complete stop (an exit speed of zero). - 2. Go over every block in chronological (forward) order and dial down junction speed values if - a. The exit speed exceeds the one forward-computed from its entry speed with the maximum allowable - acceleration over the block travel distance. - - When these stages are complete, the planner will have maximized the velocity profiles throughout the all - of the planner blocks, where every block is operating at its maximum allowable acceleration limits. In - other words, for all of the blocks in the planner, the plan is optimal and no further speed improvements - are possible. If a new block is added to the buffer, the plan is recomputed according to the said - guidelines for a new optimal plan. - - To increase computational efficiency of these guidelines, a set of planner block pointers have been - created to indicate stop-compute points for when the planner guidelines cannot logically make any further - changes or improvements to the plan when in normal operation and new blocks are streamed and added to the - planner buffer. For example, if a subset of sequential blocks in the planner have been planned and are - bracketed by junction velocities at their maximums (or by the first planner block as well), no new block - added to the planner buffer will alter the velocity profiles within them. So we no longer have to compute - them. Or, if a set of sequential blocks from the first block in the planner (or a optimal stop-compute - point) are all accelerating, they are all optimal and can not be altered by a new block added to the - planner buffer, as this will only further increase the plan speed to chronological blocks until a maximum - junction velocity is reached. However, if the operational conditions of the plan changes from infrequently - used feed holds or feedrate overrides, the stop-compute pointers will be reset and the entire plan is - recomputed as stated in the general guidelines. - - Planner buffer index mapping: - - block_buffer_tail: Points to the beginning of the planner buffer. First to be executed or being executed. - - block_buffer_head: Points to the buffer block after the last block in the buffer. Used to indicate whether - the buffer is full or empty. As described for standard ring buffers, this block is always empty. - - next_buffer_head: Points to next planner buffer block after the buffer head block. When equal to the - buffer tail, this indicates the buffer is full. - - block_buffer_planned: Points to the first buffer block after the last optimally planned block for normal - streaming operating conditions. Use for planning optimizations by avoiding recomputing parts of the - planner buffer that don't change with the addition of a new block, as describe above. - - NOTE: Since the planner only computes on what's in the planner buffer, some motions with lots of short - line segments, like G2/3 arcs or complex curves, may seem to move slow. This is because there simply isn't - enough combined distance traveled in the entire buffer to accelerate up to the nominal speed and then - decelerate to a complete stop at the end of the buffer, as stated by the guidelines. If this happens and - becomes an annoyance, there are a few simple solutions: (1) Maximize the machine acceleration. The planner - will be able to compute higher velocity profiles within the same combined distance. (2) Maximize line - segment(s) distance per block to a desired tolerance. The more combined distance the planner has to use, - the faster it can go. (3) Maximize the planner buffer size. This also will increase the combined distance - for the planner to compute over. It also increases the number of computations the planner has to perform - to compute an optimal plan, so select carefully. The Arduino 328p memory is already maxed out, but future - ARM versions should have enough memory and speed for look-ahead blocks numbering up to a hundred or more. - -*/ -static void planner_recalculate() -{ - // Initialize block index to the last block in the planner buffer. - uint8_t block_index = plan_prev_block_index(block_buffer_head); - - // Recompute plan only when there is more than one planner block in the buffer. Can't do anything with one. - if (block_index == block_buffer_tail) { - // Just set block_buffer_planned pointer. - block_buffer_planned = block_buffer_tail; - return; - } - - // Initialize planner buffer pointers and indexing. - plan_block_t *current = &block_buffer[block_index]; - - // Calculate maximum entry speed for last block in buffer, where the exit speed is always zero. - current->entry_speed_sqr = min( current->max_entry_speed_sqr, 2*current->acceleration*current->millimeters); - - // Reverse Pass: Coarsely maximize all possible deceleration curves back-planning from the last - // block in buffer. Cease planning when: (1) the last optimal planned pointer is reached. - // (2) the safe block pointer is reached, whereby the planned pointer is updated. - // NOTE: Forward pass will later refine and correct the reverse pass to create an optimal plan. - // NOTE: If the safe block is encountered before the planned block pointer, we know the safe block - // will be recomputed within the plan. So, we need to update it if it is partially completed. - float entry_speed_sqr; - plan_block_t *next; - block_index = plan_prev_block_index(block_index); - if (block_index == block_buffer_tail) { // !! OR plan pointer? Yes I think so. - // Only two plannable blocks in buffer. Compute previous block based on - // !!! May only work if a new block is being added. Not for an override. The exit speed isn't zero. - // !!! Need to make the current entry speed calculation after this. - st_update_plan_block_parameters(); - block_buffer_planned = block_buffer_tail; - } else { - // Three or more plan-able blocks - while (block_index != block_buffer_planned) { - next = current; - current = &block_buffer[block_index]; - - // Increment block index early to check if the safe block is before the current block. If encountered, - // this is an exit condition as we can't go further than this block in the reverse pass. - block_index = plan_prev_block_index(block_index); - if (block_index == block_buffer_tail) { - // Check if the safe block is partially completed. If so, update it before its exit speed - // (=current->entry speed) is over-written. - // TODO: The update breaks with feedrate overrides, because the replanning process no longer has - // the previous nominal speed to update this block with. There will need to be something along the - // lines of a nominal speed change check and send the correct value to this function. - st_update_plan_block_parameters(); - - // Set planned pointer at safe block and for loop exit after following computation is done. - block_buffer_planned = block_buffer_tail; - } - - // Compute maximum entry speed decelerating over the current block from its exit speed. - if (current->entry_speed_sqr != current->max_entry_speed_sqr) { - entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters; - if (entry_speed_sqr < current->max_entry_speed_sqr) { - current->entry_speed_sqr = entry_speed_sqr; - } else { - current->entry_speed_sqr = current->max_entry_speed_sqr; - } - } - } - } - - // Forward Pass: Forward plan the acceleration curve from the planned pointer onward. - // Also scans for optimal plan breakpoints and appropriately updates the planned pointer. - next = &block_buffer[block_buffer_planned]; // Begin at buffer planned pointer - block_index = plan_next_block_index(block_buffer_planned); - while (block_index != block_buffer_head) { - current = next; - next = &block_buffer[block_index]; - - // Any acceleration detected in the forward pass automatically moves the optimal planned - // pointer forward, since everything before this is all optimal. In other words, nothing - // can improve the plan from the buffer tail to the planned pointer by logic. - // TODO: Need to check if the planned flag logic is correct for all scenarios. It may not - // be for certain conditions. However, if the block reaches nominal speed, it can be a valid - // breakpoint substitute. - if (current->entry_speed_sqr < next->entry_speed_sqr) { - entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters; - // If true, current block is full-acceleration and we can move the planned pointer forward. - if (entry_speed_sqr < next->entry_speed_sqr) { - next->entry_speed_sqr = entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass sets this. - block_buffer_planned = block_index; // Set optimal plan pointer. - } - } - - // Any block set at its maximum entry speed also creates an optimal plan up to this - // point in the buffer. When the plan is bracketed by either the beginning of the - // buffer and a maximum entry speed or two maximum entry speeds, every block in between - // cannot logically be further improved. Hence, we don't have to recompute them anymore. - if (next->entry_speed_sqr == next->max_entry_speed_sqr) { - block_buffer_planned = block_index; // Set optimal plan pointer - } - block_index = plan_next_block_index( block_index ); - } -} - - -void plan_init() -{ - memset(&pl, 0, sizeof(pl)); // Clear planner struct - block_buffer_tail = 0; - block_buffer_head = 0; // Empty = tail - next_buffer_head = 1; // plan_next_block_index(block_buffer_head) - block_buffer_planned = 0; // = block_buffer_tail; -} - - -void plan_discard_current_block() -{ - if (block_buffer_head != block_buffer_tail) { // Discard non-empty buffer. - block_buffer_tail = plan_next_block_index( block_buffer_tail ); - } -} - - -plan_block_t *plan_get_current_block() -{ - if (block_buffer_head == block_buffer_tail) { return(NULL); } // Buffer empty - return(&block_buffer[block_buffer_tail]); -} - - -float plan_get_exec_block_exit_speed() -{ - uint8_t block_index = plan_next_block_index(block_buffer_tail); - if (block_index == block_buffer_head) { return( 0.0 ); } - return( sqrt( block_buffer[block_index].entry_speed_sqr ) ); -} - - -// Returns the availability status of the block ring buffer. True, if full. -uint8_t plan_check_full_buffer() -{ - if (block_buffer_tail == next_buffer_head) { return(true); } - return(false); -} - - -// Block until all buffered steps are executed or in a cycle state. Works with feed hold -// during a synchronize call, if it should happen. Also, waits for clean cycle end. -void plan_synchronize() -{ - while (plan_get_current_block() || sys.state == STATE_CYCLE) { - protocol_execute_runtime(); // Check and execute run-time commands - if (sys.abort) { return; } // Check for system abort - } -} - - -/* Add a new linear movement to the buffer. target[N_AXIS] is the signed, absolute target position - in millimeters. 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. - All position data passed to the planner must be in terms of machine position to keep the planner - independent of any coordinate system changes and offsets, which are handled by the g-code parser. - NOTE: Assumes buffer is available. Buffer checks are handled at a higher level by motion_control. - In other words, the buffer head is never equal to the buffer tail. Also the feed rate input value - is used in three ways: as a normal feed rate if invert_feed_rate is false, as inverse time if - invert_feed_rate is true, or as seek/rapids rate if the feed_rate value is negative (and - invert_feed_rate always false). */ -void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) -{ - // Prepare and initialize new block - plan_block_t *block = &block_buffer[block_buffer_head]; - block->step_event_count = 0; - block->millimeters = 0; - block->direction_bits = 0; - block->acceleration = SOME_LARGE_VALUE; // Scaled down to maximum acceleration later - - // Compute and store initial move distance data. - // TODO: After this for-loop, we don't touch the stepper algorithm data. Might be a good idea - // to try to keep these types of things completely separate from the planner for portability. - int32_t target_steps[N_AXIS]; - float unit_vec[N_AXIS], delta_mm; - uint8_t idx; - for (idx=0; idxsteps[idx] = labs(target_steps[idx]-pl.position[idx]); - block->step_event_count = max(block->step_event_count, block->steps[idx]); - - // Compute individual axes distance for move and prep unit vector calculations. - // NOTE: Computes true distance from converted step values. - delta_mm = (target_steps[idx] - pl.position[idx])/settings.steps_per_mm[idx]; - unit_vec[idx] = delta_mm; // Store unit vector numerator. Denominator computed later. - - // Set direction bits. Bit enabled always means direction is negative. - if (delta_mm < 0 ) { block->direction_bits |= get_direction_mask(idx); } - - // Incrementally compute total move distance by Euclidean norm. First add square of each term. - block->millimeters += delta_mm*delta_mm; - } - block->millimeters = sqrt(block->millimeters); // Complete millimeters calculation with sqrt() - - // Bail if this is a zero-length block. Highly unlikely to occur. - if (block->step_event_count == 0) { return; } - - // Adjust feed_rate value to mm/min depending on type of rate input (normal, inverse time, or rapids) - // TODO: Need to distinguish a rapids vs feed move for overrides. Some flag of some sort. - if (feed_rate < 0) { feed_rate = SOME_LARGE_VALUE; } // Scaled down to absolute max/rapids rate later - else if (invert_feed_rate) { feed_rate = block->millimeters/feed_rate; } - - // Calculate the unit vector of the line move and the block maximum feed rate and acceleration scaled - // down such that no individual axes maximum values are exceeded with respect to the line direction. - // NOTE: This calculation assumes all axes are orthogonal (Cartesian) and works with ABC-axes, - // if they are also orthogonal/independent. Operates on the absolute value of the unit vector. - float inverse_unit_vec_value; - float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple float divides - float junction_cos_theta = 0; - for (idx=0; idxacceleration = min(block->acceleration,settings.acceleration[idx]*inverse_unit_vec_value); - - // Incrementally compute cosine of angle between previous and current path. Cos(theta) of the junction - // between the current move and the previous move is simply the dot product of the two unit vectors, - // where prev_unit_vec is negative. Used later to compute maximum junction speed. - junction_cos_theta -= pl.previous_unit_vec[idx] * unit_vec[idx]; - } - } - - - // TODO: Need to check this method handling zero junction speeds when starting from rest. - if (block_buffer_head == block_buffer_tail) { - - // Initialize block entry speed as zero. Assume it will be starting from rest. Planner will correct this later. - block->entry_speed_sqr = 0.0; - block->max_junction_speed_sqr = 0.0; // Starting from rest. Enforce start from zero velocity. - - } else { - /* - Compute maximum allowable entry speed at junction by centripetal acceleration approximation. - 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 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. This approach does not actually deviate - from path, but used as a robust way to compute cornering speeds, as it takes into account the - nonlinearities of both the junction angle and junction velocity. - - NOTE: If the junction deviation value is finite, Grbl executes the motions in an exact path - mode (G61). If the junction deviation value is zero, Grbl will execute the motion in an exact - stop mode (G61.1) manner. In the future, if continuous mode (G64) is desired, the math here - is exactly the same. Instead of motioning all the way to junction point, the machine will - just follow the arc circle defined here. The Arduino doesn't have the CPU cycles to perform - a continuous mode path, but ARM-based microcontrollers most certainly do. - - NOTE: The max junction speed is a fixed value, since machine acceleration limits cannot be - changed dynamically during operation nor can the line move geometry. This must be kept in - memory in the event of a feedrate override changing the nominal speeds of blocks, which can - change the overall maximum entry speed conditions of all blocks. - */ - // NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta). - float sin_theta_d2 = sqrt(0.5*(1.0-junction_cos_theta)); // Trig half angle identity. Always positive. - - // TODO: Acceleration used in calculation needs to be limited by the minimum of the two junctions. - block->max_junction_speed_sqr = max( MINIMUM_JUNCTION_SPEED*MINIMUM_JUNCTION_SPEED, - (block->acceleration * settings.junction_deviation * sin_theta_d2)/(1.0-sin_theta_d2) ); - } - - // Store block nominal speed - block->nominal_speed_sqr = feed_rate*feed_rate; // (mm/min). Always > 0 - - // Compute the junction maximum entry based on the minimum of the junction speed and neighboring nominal speeds. - block->max_entry_speed_sqr = min(block->max_junction_speed_sqr, - min(block->nominal_speed_sqr,pl.previous_nominal_speed_sqr)); - - // Update previous path unit_vector and nominal speed (squared) - memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // pl.previous_unit_vec[] = unit_vec[] - pl.previous_nominal_speed_sqr = block->nominal_speed_sqr; - - // Update planner position - memcpy(pl.position, target_steps, sizeof(target_steps)); // pl.position[] = target_steps[] - - // New block is all set. Update buffer head and next buffer head indices. - block_buffer_head = next_buffer_head; - next_buffer_head = plan_next_block_index(block_buffer_head); - - // Finish up by recalculating the plan with the new block. - planner_recalculate(); - -// int32_t blength = block_buffer_head - block_buffer_tail; -// if (blength < 0) { blength += BLOCK_BUFFER_SIZE; } -// printInteger(blength); -} - - -// Reset the planner position vectors. Called by the system abort/initialization routine. -void plan_sync_position() -{ - uint8_t idx; - for (idx=0; idxmillimeters = (block->millimeters*step_events_remaining)/block->step_event_count; - block->step_event_count = step_events_remaining; - - // Re-plan from a complete stop. Reset planner entry speeds and buffer planned pointer. - block->entry_speed_sqr = 0.0; - block->max_entry_speed_sqr = 0.0; - block_buffer_planned = block_buffer_tail; - planner_recalculate(); -} diff --git a/archive/planner_v0_9.c b/archive/planner_v0_9.c deleted file mode 100644 index 1cf6fb4..0000000 --- a/archive/planner_v0_9.c +++ /dev/null @@ -1,476 +0,0 @@ -/* - planner.c - buffers movement commands and manages the acceleration profile plan - Part of Grbl - - Copyright (c) 2011-2013 Sungeun K. Jeon - Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2011 Jens Geisler - - 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 - (at your option) any later version. - - Grbl is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with Grbl. If not, see . -*/ - -/* The ring buffer implementation gleaned from the wiring_serial library by David A. Mellis. */ - -#include -#include -#include "planner.h" -#include "nuts_bolts.h" -#include "stepper.h" -#include "settings.h" -#include "config.h" -#include "protocol.h" - -#define SOME_LARGE_VALUE 1.0E+38 // Used by rapids and acceleration maximization calculations. Just needs - // to be larger than any feasible (mm/min)^2 or mm/sec^2 value. - -static block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instructions -static volatile uint8_t block_buffer_tail; // Index of the block to process now -static uint8_t block_buffer_head; // Index of the next block to be pushed -static uint8_t next_buffer_head; // Index of the next buffer head -static uint8_t block_buffer_planned; // Index of the optimally planned block - -// Define planner variables -typedef struct { - int32_t position[N_AXIS]; // The planner position of the tool in absolute steps. Kept separate - // from g-code position for movements requiring multiple line motions, - // i.e. arcs, canned cycles, and backlash compensation. - float previous_unit_vec[N_AXIS]; // Unit vector of previous path line segment - float previous_nominal_speed_sqr; // Nominal speed of previous path line segment - float last_target[N_AXIS]; // Target position of previous path line segment -} planner_t; -static planner_t pl; - - -// Returns the index of the next block in the ring buffer -// NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication. -static uint8_t next_block_index(uint8_t block_index) -{ - block_index++; - if (block_index == BLOCK_BUFFER_SIZE) { block_index = 0; } - return(block_index); -} - - -// Returns the index of the previous block in the ring buffer -static uint8_t prev_block_index(uint8_t block_index) -{ - if (block_index == 0) { block_index = BLOCK_BUFFER_SIZE; } - block_index--; - return(block_index); -} - - -/* STEPPER VELOCITY PROFILE DEFINITION - less than nominal rate-> + - +--------+ <- nominal_rate /|\ - / \ / | \ - initial_rate -> + \ / | + <- next->initial_rate - | + <- next->initial_rate / | | - +-------------+ initial_rate -> +----+--+ - time --> ^ ^ ^ ^ - | | | | - decelerate distance decelerate distance - - Calculates trapezoid parameters for stepper algorithm. Each block velocity profiles can be - described as either a trapezoidal or a triangular shape. The trapezoid occurs when the block - reaches the nominal speed of the block and cruises for a period of time. A triangle occurs - when the nominal speed is not reached within the block. Some other special cases exist, - such as pure ac/de-celeration velocity profiles from beginning to end or a trapezoid that - has no deceleration period when the next block resumes acceleration. - - The following function determines the type of velocity profile and stores the minimum required - information for the stepper algorithm to execute the calculated profiles. In this case, only - the new initial rate and n_steps until deceleration are computed, since the stepper algorithm - already handles acceleration and cruising and just needs to know when to start decelerating. -*/ -static void calculate_trapezoid_for_block(block_t *block, float entry_speed_sqr, float exit_speed_sqr) -{ - // Compute new initial rate for stepper algorithm - block->initial_rate = ceil(sqrt(entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) - - // TODO: Compute new nominal rate if a feedrate override occurs. - // block->nominal_rate = ceil(feed_rate*(RANADE_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) - - // Compute efficiency variable for following calculations. Removes a float divide and multiply. - // TODO: If memory allows, this can be kept in the block buffer since it doesn't change, even after feed holds. - float steps_per_mm_div_2_acc = block->step_event_count/(2*block->acceleration*block->millimeters); - - // First determine intersection distance (in steps) from the exit point for a triangular profile. - // Computes: steps_intersect = steps/mm * ( distance/2 + (v_entry^2-v_exit^2)/(4*acceleration) ) - int32_t intersect_distance = ceil( 0.5*(block->step_event_count + steps_per_mm_div_2_acc*(entry_speed_sqr-exit_speed_sqr)) ); - - // Check if this is a pure acceleration block by a intersection distance less than zero. Also - // prevents signed and unsigned integer conversion errors. - if (intersect_distance <= 0) { - block->decelerate_after = 0; - } else { - // Determine deceleration distance (in steps) from nominal speed to exit speed for a trapezoidal profile. - // Value is never negative. Nominal speed is always greater than or equal to the exit speed. - // Computes: steps_decelerate = steps/mm * ( (v_nominal^2 - v_exit^2)/(2*acceleration) ) - block->decelerate_after = ceil(steps_per_mm_div_2_acc * (block->nominal_speed_sqr - exit_speed_sqr)); - - // The lesser of the two triangle and trapezoid distances always defines the velocity profile. - if (block->decelerate_after > intersect_distance) { block->decelerate_after = intersect_distance; } - - // Finally, check if this is a pure deceleration block. - if (block->decelerate_after > block->step_event_count) { block->decelerate_after = block->step_event_count; } - } -} - - -/* PLANNER SPEED DEFINITION - +--------+ <- current->nominal_speed - / \ - current->entry_speed -> + \ - | + <- next->entry_speed - +-------------+ - time --> - - 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 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 acceleration limits. - c. The last (or newest appended) block is planned from a complete stop. - 2. Go over every block in chronological (forward) order and dial down junction speed values if - a. The speed increase within one block would require faster acceleration than the acceleration limits. - - When these stages are complete, all blocks have a junction entry speed that will allow all speed changes - to be performed using the overall limiting acceleration value, and where no junction speed is greater - than the max limit. In other words, it just computed the fastest possible velocity profile through all - buffered blocks, where the final buffered block is planned to come to a full stop when the buffer is fully - executed. Finally it will: - - 3. Convert the plan to data that the stepper algorithm needs. Only block trapezoids adjacent to a - a planner-modified junction speed with be updated, the others are assumed ok as is. - - All planner computations(1)(2) are performed in floating point to minimize numerical round-off errors. Only - when planned values are converted to stepper rate parameters(3), these are integers. If another motion block - is added while executing, the planner will re-plan and update the stored optimal velocity profile as it goes. - - Conceptually, the planner works like blowing up a balloon, where the balloon is the velocity profile. It's - constrained by the speeds at the beginning and end of the buffer, along with the maximum junction speeds and - nominal speeds of each block. Once a plan is computed, or balloon filled, this is the optimal velocity profile - through all of the motions in the buffer. Whenever a new block is added, this changes some of the limiting - conditions, or how the balloon is filled, so it has to be re-calculated to get the new optimal velocity profile. - - Also, since the planner only computes on what's in the planner buffer, some motions with lots of short line - segments, like arcs, may seem to move slow. This is because there simply isn't enough combined distance traveled - in the entire buffer to accelerate up to the nominal speed and then decelerate to a stop at the end of the - buffer. There are a few simple solutions to this: (1) Maximize the machine acceleration. The planner will be - able to compute higher speed profiles within the same combined distance. (2) Increase line segment(s) distance. - The more combined distance the planner has to use, the faster it can go. (3) Increase the MINIMUM_PLANNER_SPEED. - Not recommended. This will change what speed the planner plans to at the end of the buffer. Can lead to lost - steps when coming to a stop. (4) [BEST] Increase the planner buffer size. The more combined distance, the - bigger the balloon, or faster it can go. But this is not possible for 328p Arduinos because its limited memory - is already maxed out. Future ARM versions should not have this issue, with look-ahead planner blocks numbering - up to a hundred or more. - - NOTE: Since this function is constantly re-calculating for every new incoming block, it must be as efficient - as possible. For example, in situations like arc generation or complex curves, the short, rapid line segments - can execute faster than new blocks can be added, and the planner buffer will then starve and empty, leading - to weird hiccup-like jerky motions. -*/ -static void planner_recalculate() -{ - // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated. - uint8_t block_index = block_buffer_head; - block_t *current = &block_buffer[block_index]; // Set as last/newest block in buffer - - // Determine safe point for which to plan to. - uint8_t block_buffer_safe = next_block_index( block_buffer_tail ); - - if (block_buffer_safe == next_buffer_head) { // Only one safe block in buffer to operate on - - block_buffer_planned = block_buffer_safe; - calculate_trapezoid_for_block(current, 0.0, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED); - - } else { - - // TODO: need to account for the two block condition better. If the currently executing block - // is not safe, do we wait until its done? Can we treat the buffer head differently? - - // Calculate trapezoid for the last/newest block. - current->entry_speed_sqr = min( current->max_entry_speed_sqr, - MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED + 2*current->acceleration*current->millimeters); - calculate_trapezoid_for_block(current, current->entry_speed_sqr, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED); - - - // Reverse Pass: Back plan the deceleration curve from the last block in buffer. Cease - // planning when: (1) the last optimal planned pointer is reached. (2) the safe block - // pointer is reached, whereby the planned pointer is updated. - float entry_speed_sqr; - block_t *next; - block_index = prev_block_index(block_index); - while (block_index != block_buffer_planned) { - next = current; - current = &block_buffer[block_index]; - - // Exit loop and update planned pointer when the tail/safe block is reached. - if (block_index == block_buffer_safe) { - block_buffer_planned = block_buffer_safe; - break; - } - - // Crudely maximize deceleration curve from the end of the non-optimally planned buffer to - // the optimal plan pointer. Forward pass will adjust and finish optimizing the plan. - if (current->entry_speed_sqr != current->max_entry_speed_sqr) { - entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters; - if (entry_speed_sqr < current->max_entry_speed_sqr) { - current->entry_speed_sqr = entry_speed_sqr; - } else { - current->entry_speed_sqr = current->max_entry_speed_sqr; - } - } - block_index = prev_block_index(block_index); - } - - // Forward Pass: Forward plan the acceleration curve from the planned pointer onward. - // Also scans for optimal plan breakpoints and appropriately updates the planned pointer. - block_index = block_buffer_planned; // Begin at buffer planned pointer - next = &block_buffer[prev_block_index(block_buffer_planned)]; // Set up for while loop - while (block_index != next_buffer_head) { - current = next; - next = &block_buffer[block_index]; - - // Any acceleration detected in the forward pass automatically moves the optimal planned - // pointer forward, since everything before this is all optimal. In other words, nothing - // can improve the plan from the buffer tail to the planned pointer by logic. - if (current->entry_speed_sqr < next->entry_speed_sqr) { - block_buffer_planned = block_index; - entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters; - if (entry_speed_sqr < next->entry_speed_sqr) { - next->entry_speed_sqr = entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass set this. - } - } - - // Any block set at its maximum entry speed also creates an optimal plan up to this - // point in the buffer. The optimally planned pointer is updated. - if (next->entry_speed_sqr == next->max_entry_speed_sqr) { - block_buffer_planned = block_index; - } - - // Automatically recalculate trapezoid for all buffer blocks from last plan's optimal planned - // pointer to the end of the buffer, except the last block. - calculate_trapezoid_for_block(current, current->entry_speed_sqr, next->entry_speed_sqr); - block_index = next_block_index( block_index ); - } - - } - -} - - -void plan_init() -{ - block_buffer_tail = block_buffer_head; - next_buffer_head = next_block_index(block_buffer_head); - block_buffer_planned = block_buffer_head; - memset(&pl, 0, sizeof(pl)); // Clear planner struct -} - - -inline void plan_discard_current_block() -{ - if (block_buffer_head != block_buffer_tail) { - block_buffer_tail = next_block_index( block_buffer_tail ); - } -} - - -inline block_t *plan_get_current_block() -{ - if (block_buffer_head == block_buffer_tail) { return(NULL); } - return(&block_buffer[block_buffer_tail]); -} - - -// Returns the availability status of the block ring buffer. True, if full. -uint8_t plan_check_full_buffer() -{ - if (block_buffer_tail == next_buffer_head) { return(true); } - return(false); -} - - -// Block until all buffered steps are executed or in a cycle state. Works with feed hold -// during a synchronize call, if it should happen. Also, waits for clean cycle end. -void plan_synchronize() -{ - while (plan_get_current_block() || sys.state == STATE_CYCLE) { - protocol_execute_runtime(); // Check and execute run-time commands - if (sys.abort) { return; } // Check for system abort - } -} - - -// Add a new linear movement to the buffer. target[N_AXIS] is the signed, absolute target position -// in millimeters. 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. -// All position data passed to the planner must be in terms of machine position to keep the planner -// independent of any coordinate system changes and offsets, which are handled by the g-code parser. -// NOTE: Assumes buffer is available. Buffer checks are handled at a higher level by motion_control. -// In other words, the buffer head is never equal to the buffer tail. Also the feed rate input value -// is used in three ways: as a normal feed rate if invert_feed_rate is false, as inverse time if -// invert_feed_rate is true, or as seek/rapids rate if the feed_rate value is negative (and -// invert_feed_rate always false). -void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) -{ - // Prepare and initialize new block - block_t *block = &block_buffer[block_buffer_head]; - block->step_event_count = 0; - block->millimeters = 0; - block->direction_bits = 0; - block->acceleration = SOME_LARGE_VALUE; // Scaled down to maximum acceleration later - - // Compute and store initial move distance data. - int32_t target_steps[N_AXIS]; - float unit_vec[N_AXIS], delta_mm; - uint8_t idx; - for (idx=0; idxsteps[idx] = labs(target_steps[idx]-pl.position[idx]); - block->step_event_count = max(block->step_event_count, block->steps[idx]); - - // Compute individual axes distance for move and prep unit vector calculations. - delta_mm = target[idx] - pl.last_target[idx]; - unit_vec[idx] = delta_mm; // Store unit vector numerator. Denominator computed later. - - // Incrementally compute total move distance by Euclidean norm - block->millimeters += delta_mm*delta_mm; - - // Set direction bits. Bit enabled always means direction is negative. - if (delta_mm < 0 ) { block->direction_bits |= get_direction_mask(idx); } - } - block->millimeters = sqrt(block->millimeters); // Complete millimeters calculation - - // Bail if this is a zero-length block - if (block->step_event_count == 0) { return; } - - // Adjust feed_rate value to mm/min depending on type of rate input (normal, inverse time, or rapids) - // TODO: Need to distinguish a rapids vs feed move for overrides. Some flag of some sort. - if (feed_rate < 0) { feed_rate = SOME_LARGE_VALUE; } // Scaled down to absolute max/rapids rate later - else if (invert_feed_rate) { feed_rate = block->millimeters/feed_rate; } - - // Calculate the unit vector of the line move and the block maximum feed rate and acceleration limited - // by the maximum possible values. Block rapids rates are computed or feed rates are scaled down so - // they don't exceed the maximum axes velocities. The block acceleration is maximized based on direction - // and axes properties as well. - // NOTE: This calculation assumes all axes are orthogonal (Cartesian) and works with ABC-axes, - // if they are also orthogonal/independent. Operates on the absolute value of the unit vector. - float inverse_unit_vec_value; - float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple float divides - float junction_cos_theta = 0; - for (idx=0; idxacceleration = min(block->acceleration,settings.acceleration[idx]*inverse_unit_vec_value); - - // Incrementally compute cosine of angle between previous and current path. Cos(theta) of the junction - // between the current move and the previous move is simply the dot product of the two unit vectors, - // where prev_unit_vec is negative. Used later to compute maximum junction speed. - junction_cos_theta -= pl.previous_unit_vec[idx] * unit_vec[idx]; - } - } - - /* Compute maximum allowable entry speed at junction by centripetal acceleration approximation. - 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 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. This approach does not actually deviate - from path, but used as a robust way to compute cornering speeds, as it takes into account the - nonlinearities of both the junction angle and junction velocity. - NOTE: If the junction deviation value is finite, Grbl executes the motions in an exact path - mode (G61). If the junction deviation value is zero, Grbl will execute the motion in an exact - stop mode (G61.1) manner. In the future, if continuous mode (G64) is desired, the math here - is exactly the same. Instead of motioning all the way to junction point, the machine will - just follow the arc circle defined here. The Arduino doesn't have the CPU cycles to perform - a continuous mode path, but ARM-based microcontrollers most certainly do. - */ - // TODO: Acceleration need to be limited by the minimum of the two junctions. - // TODO: Need to setup a method to handle zero junction speeds when starting from rest. - if (block_buffer_head == block_buffer_tail) { - block->max_entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED; - } else { - // NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta). - float sin_theta_d2 = sqrt(0.5*(1.0-junction_cos_theta)); // Trig half angle identity. Always positive. - block->max_entry_speed_sqr = (block->acceleration * settings.junction_deviation * sin_theta_d2)/(1.0-sin_theta_d2); - } - - // Store block nominal speed and rate - block->nominal_speed_sqr = feed_rate*feed_rate; // (mm/min)^2. Always > 0 - block->nominal_rate = ceil(feed_rate*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) - - // Compute and store acceleration and distance traveled per step event. - block->rate_delta = ceil(block->acceleration* - ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) - block->d_next = ceil((block->millimeters*INV_TIME_MULTIPLIER)/block->step_event_count); // (mult*mm/step) - - // Update previous path unit_vector and nominal speed (squared) - memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // pl.previous_unit_vec[] = unit_vec[] - pl.previous_nominal_speed_sqr = block->nominal_speed_sqr; - - // Update planner position - memcpy(pl.position, target_steps, sizeof(target_steps)); // pl.position[] = target_steps[] - memcpy(pl.last_target, target, sizeof(target)); // pl.last_target[] = target[] - - planner_recalculate(); - - // Update buffer head and next buffer head indices. - // NOTE: The buffer head update is atomic since it's one byte. Performed after the new plan - // calculations to help prevent overwriting scenarios with adding a new block to a low buffer. - block_buffer_head = next_buffer_head; - next_buffer_head = next_block_index(block_buffer_head); -} - - -// Reset the planner position vectors. Called by the system abort/initialization routine. -void plan_sync_position() -{ - uint8_t idx; - for (idx=0; idxmillimeters = (block->millimeters*step_events_remaining)/block->step_event_count; - block->step_event_count = step_events_remaining; - - // Re-plan from a complete stop. Reset planner entry speeds and buffer planned pointer. - block->entry_speed_sqr = 0.0; - block->max_entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED; - block_buffer_planned = block_buffer_tail; - planner_recalculate(); -} diff --git a/archive/planner_v0_9.h b/archive/planner_v0_9.h deleted file mode 100644 index 84ecc4b..0000000 --- a/archive/planner_v0_9.h +++ /dev/null @@ -1,83 +0,0 @@ -/* - planner.h - buffers movement commands and manages the acceleration profile plan - Part of Grbl - - Copyright (c) 2011-2013 Sungeun K. Jeon - Copyright (c) 2009-2011 Simen Svale Skogsrud - - 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 - (at your option) any later version. - - Grbl is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with Grbl. If not, see . -*/ - -#ifndef planner_h -#define planner_h -#include "nuts_bolts.h" - -// The number of linear motions that can be in the plan at any give time -#ifndef BLOCK_BUFFER_SIZE - #define BLOCK_BUFFER_SIZE 17 -#endif - -// This struct is used when buffering the setup for each linear movement "nominal" values are as specified in -// the source g-code and may never actually be reached if acceleration management is active. -typedef struct { - - // Fields used by the bresenham algorithm for tracing the line - uint8_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) - uint32_t steps[N_AXIS]; // Step count along each axis - int32_t step_event_count; // The number of step events required to complete this block - - // Fields used by the motion planner to manage acceleration - float nominal_speed_sqr; // Axis-limit adjusted nominal speed for this block in (mm/min)^2 - float entry_speed_sqr; // Entry speed at previous-current block junction in (mm/min)^2 - float max_entry_speed_sqr; // Maximum allowable junction entry speed in (mm/min)^2 - float millimeters; // The total travel of this block in mm - float acceleration; // Axes-limit adjusted line acceleration in mm/min^2 - - // Settings for the trapezoid generator - uint32_t initial_rate; // The step rate at start of block - int32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive) - uint32_t decelerate_after; // The index of the step event on which to start decelerating - uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute - uint32_t d_next; // Scaled distance to next step - -} block_t; - -// Initialize the motion plan subsystem -void plan_init(); - -// Add a new linear movement to the buffer. target[N_AXIS] is the signed, absolute target position -// in millimeters. 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(float *target, float 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. -void plan_discard_current_block(); - -// Gets the current block. Returns NULL if buffer empty -block_t *plan_get_current_block(); - -// Reset the planner position vector (in steps) -void plan_sync_position(); - -// Reinitialize plan with a partially completed block -void plan_cycle_reinitialize(int32_t step_events_remaining); - -// Returns the status of the block ring buffer. True, if buffer is full. -uint8_t plan_check_full_buffer(); - -// Block until all buffered steps are executed -void plan_synchronize(); - -#endif diff --git a/archive/stepper_dist.c b/archive/stepper_dist.c deleted file mode 100644 index fe74e73..0000000 --- a/archive/stepper_dist.c +++ /dev/null @@ -1,706 +0,0 @@ -/* - stepper.c - stepper motor driver: executes motion plans using stepper motors - Part of Grbl - - Copyright (c) 2011-2013 Sungeun K. Jeon - Copyright (c) 2009-2011 Simen Svale Skogsrud - - 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 - (at your option) any later version. - - Grbl is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with Grbl. If not, see . -*/ - -#include -#include "stepper.h" -#include "config.h" -#include "settings.h" -#include "planner.h" -#include "nuts_bolts.h" - -// Some useful constants -#define TICKS_PER_MICROSECOND (F_CPU/1000000) - -#define RAMP_NOOP_CRUISE 0 -#define RAMP_ACCEL 1 -#define RAMP_DECEL 2 - -#define LOAD_NOOP 0 -#define LOAD_SEGMENT 1 -#define LOAD_BLOCK 2 - -#define SEGMENT_NOOP 0 -#define SEGMENT_END_OF_BLOCK bit(0) -#define RAMP_CHANGE_ACCEL bit(1) -#define RAMP_CHANGE_DECEL bit(2) - -#define MINIMUM_STEPS_PER_SEGMENT 1 // Don't change - -#define SEGMENT_BUFFER_SIZE 6 - -// Stepper state variable. Contains running data and trapezoid variables. -typedef struct { - // Used by the bresenham line algorithm - int32_t counter_x, // Counter variables for the bresenham line tracer - counter_y, - counter_z; - uint8_t segment_steps_remaining; // Steps remaining in line segment motion - - // Used by inverse time algorithm to track step rate - int32_t counter_dist; // Inverse time distance traveled since last step event - uint32_t ramp_rate; // Inverse time distance traveled per interrupt tick - uint32_t dist_per_tick; - - // Used by the stepper driver interrupt - uint8_t execute_step; // Flags step execution for each interrupt. - uint8_t step_pulse_time; // Step pulse reset time after step rise - uint8_t out_bits; // The next stepping-bits to be output - uint8_t load_flag; - - uint8_t counter_ramp; - uint8_t ramp_type; -} stepper_t; -static stepper_t st; - -// Stores stepper common data for executing steps in the segment buffer. Data can change mid-block when the -// planner updates the remaining block velocity profile with a more optimal plan or a feedrate override occurs. -// NOTE: Normally, this buffer is partially in-use, but, for the worst case scenario, it will never exceed -// the number of accessible stepper buffer segments (SEGMENT_BUFFER_SIZE-1). -typedef struct { - int32_t step_events_remaining; // Tracks step event count for the executing planner block - uint32_t dist_per_step; // Scaled distance to next step - uint32_t initial_rate; // Initialized step rate at re/start of a planner block - uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute - uint32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive) - uint32_t current_approx_rate; // Tracks the approximate segment rate to predict steps per segment to execute - int32_t decelerate_after; // Tracks when to initiate deceleration according to the planner block - float mm_per_step; -} st_data_t; -static st_data_t segment_data[SEGMENT_BUFFER_SIZE-1]; - -// Primary stepper segment ring buffer. Contains small, short line segments for the stepper algorithm to execute, -// which are "checked-out" incrementally from the first block in the planner buffer. Once "checked-out", the steps -// in the segments buffer cannot be modified by the planner, where the remaining planner block steps still can. -typedef struct { - uint8_t n_step; // Number of step events to be executed for this segment - uint8_t st_data_index; // Stepper buffer common data index. Uses this information to execute this segment. - uint8_t flag; // Stepper algorithm bit-flag for special execution conditions. -} st_segment_t; -static st_segment_t segment_buffer[SEGMENT_BUFFER_SIZE]; - -// Step segment ring buffer indices -static volatile uint8_t segment_buffer_tail; -static volatile uint8_t segment_buffer_head; -static uint8_t segment_next_head; - -static volatile uint8_t busy; // Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though. -static plan_block_t *pl_current_block; // A pointer to the planner block currently being traced -static st_segment_t *st_current_segment; -static st_data_t *st_current_data; - -// Pointers for the step segment being prepped from the planner buffer. Accessed only by the -// main program. Pointers may be planning segments or planner blocks ahead of what being executed. -static plan_block_t *pl_prep_block; // Pointer to the planner block being prepped -static st_data_t *st_prep_data; // Pointer to the stepper common data being prepped -static uint8_t pl_prep_index; // Index of planner block being prepped -static uint8_t st_data_prep_index; // Index of stepper common data block being prepped -static uint8_t pl_partial_block_flag; // Flag indicating the planner has modified the prepped planner block - - -/* __________________________ - /| |\ _________________ ^ - / | | \ /| |\ | - / | | \ / | | \ s - / | | | | | \ p - / | | | | | \ e - +-----+------------------------+---+--+---------------+----+ e - | BLOCK 1 | BLOCK 2 | d - - time -----> - - The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta - until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after - after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as - +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. -*/ - -// Stepper state initialization. Cycle should only start if the st.cycle_start flag is -// enabled. Startup init and limits call this function but shouldn't start the cycle. -void st_wake_up() -{ - // Enable steppers by resetting the stepper disable port - if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { - STEPPERS_DISABLE_PORT |= (1<> 3); - // Enable stepper driver interrupt - st.execute_step = false; - st.load_flag = LOAD_BLOCK; - - TCNT2 = 0; // Clear Timer2 - TIMSK2 |= (1<n_step; - - // If the new segment starts a new planner block, initialize stepper variables and counters. - // NOTE: For new segments only, the step counters are not updated to ensure step phasing is continuous. - if (st.load_flag == LOAD_BLOCK) { - pl_current_block = plan_get_current_block(); // Should always be there. Stepper buffer handles this. - st_current_data = &segment_data[segment_buffer[segment_buffer_tail].st_data_index]; - - // Initialize direction bits for block. Set execute flag to set directions bits upon next ISR tick. - st.out_bits = pl_current_block->direction_bits ^ settings.invert_mask; - st.execute_step = true; - - // Initialize Bresenham line counters - st.counter_x = (pl_current_block->step_event_count >> 1); - st.counter_y = st.counter_x; - st.counter_z = st.counter_x; - - // Initialize inverse time, step rate data, and acceleration ramp counters - st.counter_dist = st_current_data->dist_per_step; // dist_per_step always greater than ramp_rate. - st.ramp_rate = st_current_data->initial_rate; - st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK/2; // Initialize ramp counter via midpoint rule - st.ramp_type = RAMP_NOOP_CRUISE; // Initialize as no ramp operation. Corrected later if necessary. - - // Ensure the initial step rate exceeds the MINIMUM_STEP_RATE. - if (st.ramp_rate < MINIMUM_STEP_RATE) { st.dist_per_tick = MINIMUM_STEP_RATE; } - else { st.dist_per_tick = st.ramp_rate; } - } - - // Check if ramp conditions have changed. If so, update ramp counters and control variables. - if ( st_current_segment->flag & (RAMP_CHANGE_DECEL | RAMP_CHANGE_ACCEL) ) { - /* Compute correct ramp count for a ramp change. Upon a switch from acceleration to deceleration, - or vice-versa, the new ramp count must be set to trigger the next acceleration tick equal to - the number of ramp ISR ticks counted since the last acceleration tick. This is ensures the - ramp is executed exactly as the plan dictates. Otherwise, when a ramp begins from a known - rate (nominal/cruise or initial), the ramp count must be set to ISR_TICKS_PER_ACCELERATION_TICK/2 - as mandated by the mid-point rule. For the latter conditions, the ramp count have been - initialized such that the following computation is still correct. */ - st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK-st.counter_ramp; - if ( st_current_segment->flag & RAMP_CHANGE_DECEL ) { st.ramp_type = RAMP_DECEL; } - else { st.ramp_type = RAMP_ACCEL; } - } - - st.load_flag = LOAD_NOOP; // Segment motion loaded. Set no-operation flag to skip during execution. - - } else { - // Can't discard planner block here if a feed hold stops in middle of block. - st_go_idle(); - bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end - return; // Nothing to do but exit. - } - - } - - // Adjust inverse time counter for ac/de-celerations - if (st.ramp_type) { // Ignored when ramp type is RAMP_NOOP_CRUISE - st.counter_ramp--; // Tick acceleration ramp counter - if (st.counter_ramp == 0) { // Adjust step rate when its time - st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter - if (st.ramp_type == RAMP_ACCEL) { // Adjust velocity for acceleration - st.ramp_rate += st_current_data->rate_delta; - if (st.ramp_rate >= st_current_data->nominal_rate) { // Reached nominal rate. - st.ramp_rate = st_current_data->nominal_rate; // Set cruising velocity - st.ramp_type = RAMP_NOOP_CRUISE; // Set ramp flag to cruising - st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK/2; // Re-initialize counter for next ramp change. - } - } else { // Adjust velocity for deceleration. - if (st.ramp_rate > st_current_data->rate_delta) { - st.ramp_rate -= st_current_data->rate_delta; - } else { // Moving near zero feed rate. Gracefully slow down. - st.ramp_rate >>= 1; // Integer divide by 2 until complete. Also prevents overflow. - } - } - // Adjust for minimum step rate, but retain operating ramp rate for accurate velocity tracing. - if (st.ramp_rate < MINIMUM_STEP_RATE) { st.dist_per_tick = MINIMUM_STEP_RATE; } - else { st.dist_per_tick = st.ramp_rate; } - } - } - - // Iterate inverse time counter. Triggers each Bresenham step event. - st.counter_dist -= st.dist_per_tick; - - // Execute Bresenham step event, when it's time to do so. - if (st.counter_dist < 0) { - st.counter_dist += st_current_data->dist_per_step; // Reload inverse time counter - - st.out_bits = pl_current_block->direction_bits; // Reset out_bits and reload direction bits - st.execute_step = true; - - // Execute step displacement profile by Bresenham line algorithm - st.counter_x -= pl_current_block->steps[X_AXIS]; - if (st.counter_x < 0) { - st.out_bits |= (1<step_event_count; - if (st.out_bits & (1<steps[Y_AXIS]; - if (st.counter_y < 0) { - st.out_bits |= (1<step_event_count; - if (st.out_bits & (1<steps[Z_AXIS]; - if (st.counter_z < 0) { - st.out_bits |= (1<step_event_count; - if (st.out_bits & (1<flag & SEGMENT_END_OF_BLOCK) { - plan_discard_current_block(); - st.load_flag = LOAD_BLOCK; - } else { - st.load_flag = LOAD_SEGMENT; - } - - // Discard current segment by advancing buffer tail index - if ( ++segment_buffer_tail == SEGMENT_BUFFER_SIZE) { segment_buffer_tail = 0; } - } - - st.out_bits ^= settings.invert_mask; // Apply step port invert mask - } - busy = false; -// SPINDLE_ENABLE_PORT ^= 1<step_events_remaining); -// st.ramp_type = RAMP_ACCEL; -// st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK/2; -// st.ramp_rate = 0; -// sys.state = STATE_QUEUED; -// } else { -// sys.state = STATE_IDLE; -// } - sys.state = STATE_IDLE; - -} - - -/* Prepares step segment buffer. Continuously called from main program. - - The segment buffer is an intermediary buffer interface between the execution of steps - by the stepper algorithm and the velocity profiles generated by the planner. The stepper - algorithm only executes steps within the segment buffer and is filled by the main program - when steps are "checked-out" from the first block in the planner buffer. This keeps the - step execution and planning optimization processes atomic and protected from each other. - The number of steps "checked-out" from the planner buffer and the number of segments in - the segment buffer is sized and computed such that no operation in the main program takes - longer than the time it takes the stepper algorithm to empty it before refilling it. - Currently, the segment buffer conservatively holds roughly up to 40-60 msec of steps. - - NOTE: The segment buffer executes a set number of steps over an approximate time period. - If we try to execute over a fixed time period, it is difficult to guarantee or predict - how many steps will execute over it, especially when the step pulse phasing between the - neighboring segments must also be kept consistent. Meaning that, if the last segment step - pulses right before a segment end, the next segment must delay its first pulse so that the - step pulses are consistently spaced apart over time to keep the step pulse train nice and - smooth. Keeping track of phasing and ensuring that the exact number of steps are executed - as defined by the planner block, the related computational overhead can get quickly and - prohibitively expensive, especially in real-time. - Since the stepper algorithm automatically takes care of the step pulse phasing with - its ramp and inverse time counters by retaining the count remainders, we don't have to - explicitly and expensively track and synchronize the exact number of steps, time, and - phasing of steps. All we need to do is approximate the number of steps in each segment - such that the segment buffer has enough execution time for the main program to do what - it needs to do and refill it when it comes back. In other words, we just need to compute - a cheap approximation of the current velocity and the number of steps over it. -*/ - -/* - TODO: Figure out how to enforce a deceleration when a feedrate override is reduced. - The problem is that when an override is reduced, the planner may not plan back to - the current rate. Meaning that the velocity profiles for certain conditions no longer - are trapezoidal or triangular. For example, if the current block is cruising at a - nominal rate and the feedrate override is reduced, the new nominal rate will now be - lower. The velocity profile must first decelerate to the new nominal rate and then - follow on the new plan. So the remaining velocity profile will have a decelerate, - cruise, and another decelerate. - Another issue is whether or not a feedrate override reduction causes a deceleration - that acts over several planner blocks. For example, say that the plan is already - heavily decelerating throughout it, reducing the feedrate will not do much to it. So, - how do we determine when to resume the new plan? How many blocks do we have to wait - until the new plan intersects with the deceleration curve? One plus though, the - deceleration will never be more than the number of blocks in the entire planner buffer, - but it theoretically can be equal to it when all planner blocks are decelerating already. -*/ -void st_prep_buffer() -{ - if (sys.state == STATE_QUEUED) { return; } // Block until a motion state is issued - while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer. - - // Initialize new segment - st_segment_t *prep_segment = &segment_buffer[segment_buffer_head]; - prep_segment->flag = SEGMENT_NOOP; - - // Determine if we need to load a new planner block. - if (pl_prep_block == NULL) { - pl_prep_block = plan_get_block_by_index(pl_prep_index); // Query planner for a queued block - if (pl_prep_block == NULL) { return; } // No planner blocks. Exit. - - -SPINDLE_ENABLE_PORT ^= 1<step_events_remaining = last_st_prep_data->step_events_remaining; - st_prep_data->rate_delta = last_st_prep_data->rate_delta; - st_prep_data->dist_per_step = last_st_prep_data->dist_per_step; - st_prep_data->nominal_rate = last_st_prep_data->nominal_rate; // TODO: Feedrate overrides recomputes this. - - st_prep_data->mm_per_step = last_st_prep_data->mm_per_step; - - pl_partial_block_flag = false; // Reset flag - - } else { - - // Prepare commonly shared planner block data for the ensuing segment buffer moves ad-hoc, since - // the planner buffer can dynamically change the velocity profile data as blocks are added. - st_prep_data = &segment_data[st_data_prep_index]; - - // Initialize Bresenham variables - st_prep_data->step_events_remaining = pl_prep_block->step_event_count; - - // Convert planner block velocity profile data to stepper rate and step distance data. - st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) - st_prep_data->rate_delta = ceil(pl_prep_block->acceleration* - ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) - st_prep_data->dist_per_step = ceil((pl_prep_block->millimeters*INV_TIME_MULTIPLIER)/pl_prep_block->step_event_count); // (mult*mm/step) - - // TODO: Check if we really need to store this. - st_prep_data->mm_per_step = pl_prep_block->millimeters/pl_prep_block->step_event_count; - - } - - // Convert planner entry speed to stepper initial rate. - st_prep_data->initial_rate = ceil(sqrt(pl_prep_block->entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) - - // TODO: Nominal rate changes with feedrate override. - // st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) - - st_prep_data->current_approx_rate = st_prep_data->initial_rate; - - // Calculate the planner block velocity profile type, determine deceleration point, and initial ramp. - float mm_decelerate_after = plan_calculate_velocity_profile(pl_prep_index); - st_prep_data->decelerate_after = ceil( mm_decelerate_after/st_prep_data->mm_per_step ); - if (st_prep_data->decelerate_after > 0) { // If 0, RAMP_CHANGE_DECEL flag is set later. - if (st_prep_data->initial_rate != st_prep_data->nominal_rate) { prep_segment->flag = RAMP_CHANGE_ACCEL; } - } - } - - // Set new segment to point to the current segment data block. - prep_segment->st_data_index = st_data_prep_index; - - // Approximate the velocity over the new segment using the already computed rate values. - // NOTE: This assumes that each segment will have an execution time roughly equal to every ACCELERATION_TICK. - // We do this to minimize memory and computational requirements. However, this could easily be replaced with - // a more exact approximation or have an user-defined time per segment, if CPU and memory overhead allows. - if (st_prep_data->decelerate_after <= 0) { - if (st_prep_data->decelerate_after == 0) { prep_segment->flag = RAMP_CHANGE_DECEL; } // Set segment deceleration flag - else { st_prep_data->current_approx_rate -= st_prep_data->rate_delta; } - if (st_prep_data->current_approx_rate < st_prep_data->rate_delta) { st_prep_data->current_approx_rate >>= 1; } - } else { - if (st_prep_data->current_approx_rate < st_prep_data->nominal_rate) { - st_prep_data->current_approx_rate += st_prep_data->rate_delta; - if (st_prep_data->current_approx_rate > st_prep_data->nominal_rate) { - st_prep_data->current_approx_rate = st_prep_data->nominal_rate; - } - } - } - - // TODO: Look into replacing the following dist_per_step divide with multiplying its inverse to save cycles. - - // Compute the number of steps in the prepped segment based on the approximate current rate. - // NOTE: The dist_per_step divide cancels out the INV_TIME_MULTIPLIER and converts the rate value to steps. - prep_segment->n_step = ceil(max(MINIMUM_STEP_RATE,st_prep_data->current_approx_rate)* - (ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND)/st_prep_data->dist_per_step); - // NOTE: Ensures it moves for very slow motions, but MINIMUM_STEP_RATE should always set this too. Perhaps - // a compile-time check to see if MINIMUM_STEP_RATE is set high enough is all that is needed. - prep_segment->n_step = max(prep_segment->n_step,MINIMUM_STEPS_PER_SEGMENT); - // NOTE: As long as the ACCELERATION_TICKS_PER_SECOND is valid, n_step should never exceed 255 and overflow. - // prep_segment->n_step = min(prep_segment->n_step,MAXIMUM_STEPS_PER_BLOCK); // Prevent unsigned int8 overflow. - - // Check if n_step exceeds steps remaining in planner block. If so, truncate. - if (prep_segment->n_step > st_prep_data->step_events_remaining) { - prep_segment->n_step = st_prep_data->step_events_remaining; - } - - // Check if n_step crosses decelerate point in block. If so, truncate to ensure the deceleration - // ramp counters are set correctly during execution. - if (st_prep_data->decelerate_after > 0) { - if (prep_segment->n_step > st_prep_data->decelerate_after) { - prep_segment->n_step = st_prep_data->decelerate_after; - } - } - - // Update stepper common data variables. - st_prep_data->decelerate_after -= prep_segment->n_step; - st_prep_data->step_events_remaining -= prep_segment->n_step; - - // Check for end of planner block - if ( st_prep_data->step_events_remaining == 0 ) { - - // TODO: When a feed hold ends, the step_events_remaining will also be zero, even though a block - // have partially been completed. We need to flag the stepper algorithm to indicate a stepper shutdown - // when complete, but not remove the planner block unless it truly is the end of the block (rare). - - // Set EOB bitflag so stepper algorithm discards the planner block after this segment completes. - prep_segment->flag |= SEGMENT_END_OF_BLOCK; - // Move planner pointer to next block and flag to load a new block for the next segment. - pl_prep_index = plan_next_block_index(pl_prep_index); - pl_prep_block = NULL; - } - - // New step segment completed. Increment segment buffer indices. - segment_buffer_head = segment_next_head; - if ( ++segment_next_head == SEGMENT_BUFFER_SIZE ) { segment_next_head = 0; } - -// long a = prep_segment->n_step; -// printInteger(a); -// printString(" "); -SPINDLE_ENABLE_PORT ^= 1<step_events_remaining*st_prep_data->mm_per_step; - if (st_prep_data->decelerate_after > 0) { *is_decelerating = false; } - else { *is_decelerating = true; } - - // Flag for new prep_block when st_prep_buffer() is called after the planner recomputes. - pl_partial_block_flag = true; - pl_prep_block = NULL; - } - return; -} diff --git a/archive/stepper_old.c b/archive/stepper_old.c deleted file mode 100644 index d702eba..0000000 --- a/archive/stepper_old.c +++ /dev/null @@ -1,746 +0,0 @@ -/* - stepper.c - stepper motor driver: executes motion plans using stepper motors - Part of Grbl - - Copyright (c) 2011-2013 Sungeun K. Jeon - Copyright (c) 2009-2011 Simen Svale Skogsrud - - 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 - (at your option) any later version. - - Grbl is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with Grbl. If not, see . -*/ - -#include -#include "stepper.h" -#include "config.h" -#include "settings.h" -#include "planner.h" -#include "nuts_bolts.h" - -// Some useful constants -#define TICKS_PER_MICROSECOND (F_CPU/1000000) - -#define RAMP_NOOP_CRUISE 0 -#define RAMP_ACCEL 1 -#define RAMP_DECEL 2 - -#define LOAD_NOOP 0 -#define LOAD_SEGMENT 1 -#define LOAD_BLOCK 2 - -#define SEGMENT_NOOP 0 -#define SEGMENT_END_OF_BLOCK bit(0) -#define SEGMENT_ACCEL bit(1) -#define SEGMENT_DECEL bit(2) - -#define MINIMUM_STEPS_PER_SEGMENT 1 // Don't change - -#define SEGMENT_BUFFER_SIZE 6 - -// Stepper state variable. Contains running data and trapezoid variables. -typedef struct { - // Used by the bresenham line algorithm - int32_t counter_x, // Counter variables for the bresenham line tracer - counter_y, - counter_z; - uint8_t segment_steps_remaining; // Steps remaining in line segment motion - - // Used by inverse time algorithm to track step rate - int32_t counter_d; // Inverse time distance traveled since last step event - uint32_t delta_d; // Inverse time distance traveled per interrupt tick - uint32_t d_per_tick; - - // Used by the stepper driver interrupt - uint8_t execute_step; // Flags step execution for each interrupt. - uint8_t step_pulse_time; // Step pulse reset time after step rise - uint8_t out_bits; // The next stepping-bits to be output - uint8_t load_flag; - - uint8_t ramp_count; - uint8_t ramp_type; -} stepper_t; -static stepper_t st; - -// Stores stepper common data for executing steps in the segment buffer. Data can change mid-block when the -// planner updates the remaining block velocity profile with a more optimal plan or a feedrate override occurs. -// NOTE: Normally, this buffer is partially in-use, but, for the worst case scenario, it will never exceed -// the number of accessible stepper buffer segments (SEGMENT_BUFFER_SIZE-1). -typedef struct { - int32_t step_events_remaining; // Tracks step event count for the executing planner block - uint32_t d_next; // Scaled distance to next step - uint32_t initial_rate; // Initialized step rate at re/start of a planner block - uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute - uint32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive) - uint32_t current_approx_rate; // Tracks the approximate segment rate to predict steps per segment to execute - int32_t decelerate_after; // Tracks when to initiate deceleration according to the planner block - float mm_per_step; -} st_data_t; -static st_data_t segment_data[SEGMENT_BUFFER_SIZE-1]; - -// Primary stepper segment ring buffer. Contains small, short line segments for the stepper algorithm to execute, -// which are "checked-out" incrementally from the first block in the planner buffer. Once "checked-out", the steps -// in the segments buffer cannot be modified by the planner, where the remaining planner block steps still can. -typedef struct { - uint8_t n_step; // Number of step events to be executed for this segment - uint8_t st_data_index; // Stepper buffer common data index. Uses this information to execute this segment. - uint8_t flag; // Stepper algorithm bit-flag for special execution conditions. -} st_segment_t; -static st_segment_t segment_buffer[SEGMENT_BUFFER_SIZE]; - -// Step segment ring buffer indices -static volatile uint8_t segment_buffer_tail; -static volatile uint8_t segment_buffer_head; -static uint8_t segment_next_head; - -static volatile uint8_t busy; // Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though. -static plan_block_t *pl_current_block; // A pointer to the planner block currently being traced -static st_segment_t *st_current_segment; -static st_data_t *st_current_data; - -// Pointers for the step segment being prepped from the planner buffer. Accessed only by the -// main program. Pointers may be planning segments or planner blocks ahead of what being executed. -static plan_block_t *pl_prep_block; // Pointer to the planner block being prepped -static st_data_t *st_prep_data; // Pointer to the stepper common data being prepped -static uint8_t pl_prep_index; // Index of planner block being prepped -static uint8_t st_data_prep_index; // Index of stepper common data block being prepped -static uint8_t pl_partial_block_flag; // Flag indicating the planner has modified the prepped planner block - - -/* __________________________ - /| |\ _________________ ^ - / | | \ /| |\ | - / | | \ / | | \ s - / | | | | | \ p - / | | | | | \ e - +-----+------------------------+---+--+---------------+----+ e - | BLOCK 1 | BLOCK 2 | d - - time -----> - - The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta - until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after - after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as - +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. -*/ - -// Stepper state initialization. Cycle should only start if the st.cycle_start flag is -// enabled. Startup init and limits call this function but shouldn't start the cycle. -void st_wake_up() -{ - // Enable steppers by resetting the stepper disable port - if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { - STEPPERS_DISABLE_PORT |= (1<> 3); - // Enable stepper driver interrupt - st.execute_step = false; - st.load_flag = LOAD_BLOCK; - - TCNT2 = 0; // Clear Timer2 - TIMSK2 |= (1<n_step; - - // Check if the counters need to be reset for a new planner block - if (st.load_flag == LOAD_BLOCK) { - pl_current_block = plan_get_current_block(); // Should always be there. Stepper buffer handles this. - st_current_data = &segment_data[segment_buffer[segment_buffer_tail].st_data_index]; //st_current_segment->st_data_index]; - - // Initialize direction bits for block - st.out_bits = pl_current_block->direction_bits ^ settings.invert_mask; - st.execute_step = true; // Set flag to set direction bits upon next ISR tick. - - // Initialize Bresenham line counters - st.counter_x = (pl_current_block->step_event_count >> 1); - st.counter_y = st.counter_x; - st.counter_z = st.counter_x; - - // Initialize inverse time and step rate counter data - st.counter_d = st_current_data->d_next; // d_next always greater than delta_d. - if (st.delta_d < MINIMUM_STEP_RATE) { st.d_per_tick = MINIMUM_STEP_RATE; } - else { st.d_per_tick = st.delta_d; } - - // During feed hold, do not update rate, ramp type, or ramp counters. Keep decelerating. -// if (sys.state == STATE_CYCLE) { - st.delta_d = st_current_data->initial_rate; - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Initialize ramp counter via midpoint rule - st.ramp_type = RAMP_NOOP_CRUISE; // Initialize as no ramp operation. Corrected later if necessary. -// } - - } - - // Acceleration and cruise handled by ramping. Just check if deceleration needs to begin. - if ( st_current_segment->flag & (SEGMENT_DECEL | SEGMENT_ACCEL) ) { - /* Compute correct ramp count for a ramp change. Upon a switch from acceleration to deceleration, - or vice-versa, the new ramp count must be set to trigger the next acceleration tick equal to - the number of ramp ISR ticks counted since the last acceleration tick. This is ensures the - ramp is executed exactly as the plan dictates. Otherwise, when a ramp begins from a known - rate (nominal/cruise or initial), the ramp count must be set to ISR_TICKS_PER_ACCELERATION_TICK/2 - as mandated by the mid-point rule. For these conditions, the ramp count have been initialized - such that the following computation is still correct. */ - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; - if ( st_current_segment->flag & SEGMENT_DECEL ) { st.ramp_type = RAMP_DECEL; } - else { st.ramp_type = RAMP_ACCEL; } - } - - st.load_flag = LOAD_NOOP; // Segment motion loaded. Set no-operation flag to skip during execution. - - } else { - // Can't discard planner block here if a feed hold stops in middle of block. - st_go_idle(); - bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end - return; // Nothing to do but exit. - } - - } - - // Adjust inverse time counter for ac/de-celerations - // NOTE: Accelerations are handled by the stepper algorithm as it's thought to be more computationally - // efficient on the Arduino AVR. This could may not be true with higher ISR frequencies or faster CPUs. - if (st.ramp_type) { // Ignored when ramp type is NOOP_CRUISE - st.ramp_count--; // Tick acceleration ramp counter - if (st.ramp_count == 0) { // Adjust step rate when its time - if (st.ramp_type == RAMP_ACCEL) { // Adjust velocity for acceleration - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter - st.delta_d += st_current_data->rate_delta; - if (st.delta_d >= st_current_data->nominal_rate) { // Reached nominal rate. - st.delta_d = st_current_data->nominal_rate; // Set cruising velocity - st.ramp_type = RAMP_NOOP_CRUISE; // Set ramp flag to cruising - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Re-initialize counter for next ramp. - } - } else { // Adjust velocity for deceleration. - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter - if (st.delta_d > st_current_data->rate_delta) { - st.delta_d -= st_current_data->rate_delta; - } else { // Moving near zero feed rate. Gracefully slow down. - st.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow. - - // TODO: Check for and handle feed hold exit? At this point, machine is stopped. - // - Set system flag to recompute plan and reset segment buffer. - // - Segment steps in buffer needs to be returned to planner correctly. - // busy = false; - // return; - - } - } - // Finalize adjusted step rate. Ensure minimum. - if (st.delta_d < MINIMUM_STEP_RATE) { st.d_per_tick = MINIMUM_STEP_RATE; } - else { st.d_per_tick = st.delta_d; } - } - } - - // Iterate inverse time counter. Triggers each Bresenham step event. - st.counter_d -= st.d_per_tick; - - // Execute Bresenham step event, when it's time to do so. - if (st.counter_d < 0) { - st.counter_d += st_current_data->d_next; // Reload inverse time counter - - st.out_bits = pl_current_block->direction_bits; // Reset out_bits and reload direction bits - st.execute_step = true; - - // Execute step displacement profile by Bresenham line algorithm - st.counter_x -= pl_current_block->steps[X_AXIS]; - if (st.counter_x < 0) { - st.out_bits |= (1<step_event_count; - // st.steps_x++; - if (st.out_bits & (1<steps[Y_AXIS]; - if (st.counter_y < 0) { - st.out_bits |= (1<step_event_count; - // st.steps_y++; - if (st.out_bits & (1<steps[Z_AXIS]; - if (st.counter_z < 0) { - st.out_bits |= (1<step_event_count; - // st.steps_z++; - if (st.out_bits & (1< 0) { - if (st.out_bits & (1< 0) { - if (st.out_bits & (1< 0) { - if (st.out_bits & (1<flag & SEGMENT_END_OF_BLOCK) { - plan_discard_current_block(); - st.load_flag = LOAD_BLOCK; - } else { - st.load_flag = LOAD_SEGMENT; - } - - // Discard current segment by advancing buffer tail index - if ( ++segment_buffer_tail == SEGMENT_BUFFER_SIZE) { segment_buffer_tail = 0; } - - } - - st.out_bits ^= settings.invert_mask; // Apply step port invert mask - } - busy = false; -// SPINDLE_ENABLE_PORT ^= 1<step_events_remaining); -// st.ramp_type = RAMP_ACCEL; -// st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; -// st.delta_d = 0; -// sys.state = STATE_QUEUED; -// } else { -// sys.state = STATE_IDLE; -// } - sys.state = STATE_IDLE; - -} - - -/* Prepares step segment buffer. Continuously called from main program. - - The segment buffer is an intermediary buffer interface between the execution of steps - by the stepper algorithm and the velocity profiles generated by the planner. The stepper - algorithm only executes steps within the segment buffer and is filled by the main program - when steps are "checked-out" from the first block in the planner buffer. This keeps the - step execution and planning optimization processes atomic and protected from each other. - The number of steps "checked-out" from the planner buffer and the number of segments in - the segment buffer is sized and computed such that no operation in the main program takes - longer than the time it takes the stepper algorithm to empty it before refilling it. - Currently, the segment buffer conservatively holds roughly up to 40-60 msec of steps. - - NOTE: The segment buffer executes a set number of steps over an approximate time period. - If we try to execute over a set time period, it is difficult to guarantee or predict how - many steps will execute over it, especially when the step pulse phasing between the - neighboring segments are kept consistent. Meaning that, if the last segment step pulses - right before its end, the next segment must delay its first pulse so that the step pulses - are consistently spaced apart over time to keep the step pulse train nice and smooth. - Keeping track of phasing and ensuring that the exact number of steps are executed as - defined by the planner block, the related computational overhead gets quickly and - prohibitively expensive, especially in real-time. - Since the stepper algorithm automatically takes care of the step pulse phasing with - its ramp and inverse time counters, we don't have to explicitly and expensively track the - exact number of steps, time, or phasing of steps. All we need to do is approximate - the number of steps in each segment such that the segment buffer has enough execution time - for the main program to do what it needs to do and refill it when it has time. In other - words, we just need to compute a cheap approximation of the current velocity and the - number of steps over it. -*/ - -/* - TODO: Figure out how to enforce a deceleration when a feedrate override is reduced. - The problem is that when an override is reduced, the planner may not plan back to - the current rate. Meaning that the velocity profiles for certain conditions no longer - are trapezoidal or triangular. For example, if the current block is cruising at a - nominal rate and the feedrate override is reduced, the new nominal rate will now be - lower. The velocity profile must first decelerate to the new nominal rate and then - follow on the new plan. So the remaining velocity profile will have a decelerate, - cruise, and another decelerate. - Another issue is whether or not a feedrate override reduction causes a deceleration - that acts over several planner blocks. For example, say that the plan is already - heavily decelerating throughout it, reducing the feedrate will not do much to it. So, - how do we determine when to resume the new plan? How many blocks do we have to wait - until the new plan intersects with the deceleration curve? One plus though, the - deceleration will never be more than the number of blocks in the entire planner buffer, - but it theoretically can be equal to it when all planner blocks are decelerating already. -*/ -void st_prep_buffer() -{ - while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer. - - // Initialize new segment - st_segment_t *prep_segment = &segment_buffer[segment_buffer_head]; - prep_segment->flag = SEGMENT_NOOP; - - // Determine if we need to load a new planner block. - if (pl_prep_block == NULL) { - pl_prep_block = plan_get_block_by_index(pl_prep_index); // Query planner for a queued block - if (pl_prep_block == NULL) { return; } // No planner blocks. Exit. - - // Increment stepper common data index - if ( ++st_data_prep_index == (SEGMENT_BUFFER_SIZE-1) ) { st_data_prep_index = 0; } - - // Check if the planner has re-computed this block mid-execution. If so, push the previous segment - // data. Otherwise, prepare a new segment data for the new planner block. - if (pl_partial_block_flag) { - - // Prepare new shared segment block data and copy the relevant last segment block data. - st_data_t *last_st_prep_data; - last_st_prep_data = st_prep_data; - st_prep_data = &segment_data[st_data_prep_index]; - - st_prep_data->step_events_remaining = last_st_prep_data->step_events_remaining; - st_prep_data->rate_delta = last_st_prep_data->rate_delta; - st_prep_data->d_next = last_st_prep_data->d_next; - st_prep_data->nominal_rate = last_st_prep_data->nominal_rate; // TODO: Feedrate overrides recomputes this. - - st_prep_data->mm_per_step = last_st_prep_data->mm_per_step; - - pl_partial_block_flag = false; // Reset flag - - } else { - - // Prepare commonly shared planner block data for the ensuing segment buffer moves ad-hoc, since - // the planner buffer can dynamically change the velocity profile data as blocks are added. - st_prep_data = &segment_data[st_data_prep_index]; - - // Initialize Bresenham variables - st_prep_data->step_events_remaining = pl_prep_block->step_event_count; - - // Convert planner block velocity profile data to stepper rate and step distance data. - st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) - st_prep_data->rate_delta = ceil(pl_prep_block->acceleration* - ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) - st_prep_data->d_next = ceil((pl_prep_block->millimeters*INV_TIME_MULTIPLIER)/pl_prep_block->step_event_count); // (mult*mm/step) - - // TODO: Check if we really need to store this. - st_prep_data->mm_per_step = pl_prep_block->millimeters/pl_prep_block->step_event_count; - - } - - // Convert planner entry speed to stepper initial rate. - st_prep_data->initial_rate = ceil(sqrt(pl_prep_block->entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) - - // TODO: Nominal rate changes with feedrate override. - // st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) - - st_prep_data->current_approx_rate = st_prep_data->initial_rate; - - // Calculate the planner block velocity profile type, determine deceleration point, and initial ramp. - float mm_decelerate_after = plan_calculate_velocity_profile(pl_prep_index); - st_prep_data->decelerate_after = ceil( mm_decelerate_after/st_prep_data->mm_per_step ); - if (st_prep_data->decelerate_after > 0) { // If 0, SEGMENT_DECEL flag is set later. - if (st_prep_data->initial_rate != st_prep_data->nominal_rate) { prep_segment->flag = SEGMENT_ACCEL; } - } - } - - // Set new segment to point to the current segment data block. - prep_segment->st_data_index = st_data_prep_index; - - // Approximate the velocity over the new segment using the already computed rate values. - // NOTE: This assumes that each segment will have an execution time roughly equal to every ACCELERATION_TICK. - // We do this to minimize memory and computational requirements. However, this could easily be replaced with - // a more exact approximation or have a unique time per segment, if CPU and memory overhead allows. - if (st_prep_data->decelerate_after <= 0) { - if (st_prep_data->decelerate_after == 0) { prep_segment->flag = SEGMENT_DECEL; } // Set segment deceleration flag - else { st_prep_data->current_approx_rate -= st_prep_data->rate_delta; } - if (st_prep_data->current_approx_rate < st_prep_data->rate_delta) { st_prep_data->current_approx_rate >>= 1; } - } else { - if (st_prep_data->current_approx_rate < st_prep_data->nominal_rate) { - st_prep_data->current_approx_rate += st_prep_data->rate_delta; - if (st_prep_data->current_approx_rate > st_prep_data->nominal_rate) { - st_prep_data->current_approx_rate = st_prep_data->nominal_rate; - } - } - } - - // Compute the number of steps in the prepped segment based on the approximate current rate. - // NOTE: The d_next divide cancels out the INV_TIME_MULTIPLIER and converts the rate value to steps. - prep_segment->n_step = ceil(max(MINIMUM_STEP_RATE,st_prep_data->current_approx_rate)* - (ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND)/st_prep_data->d_next); - // NOTE: Ensures it moves for very slow motions, but MINIMUM_STEP_RATE should always set this too. Perhaps - // a compile-time check to see if MINIMUM_STEP_RATE is set high enough is all that is needed. - prep_segment->n_step = max(prep_segment->n_step,MINIMUM_STEPS_PER_SEGMENT); - // NOTE: As long as the ACCELERATION_TICKS_PER_SECOND is valid, n_step should never exceed 255 and overflow. - // prep_segment->n_step = min(prep_segment->n_step,MAXIMUM_STEPS_PER_BLOCK); // Prevent unsigned int8 overflow. - - // Check if n_step exceeds steps remaining in planner block. If so, truncate. - if (prep_segment->n_step > st_prep_data->step_events_remaining) { - prep_segment->n_step = st_prep_data->step_events_remaining; - } - - // Check if n_step crosses decelerate point in block. If so, truncate to ensure the deceleration - // ramp counters are set correctly during execution. - if (st_prep_data->decelerate_after > 0) { - if (prep_segment->n_step > st_prep_data->decelerate_after) { - prep_segment->n_step = st_prep_data->decelerate_after; - } - } - - // Update stepper common data variables. - st_prep_data->decelerate_after -= prep_segment->n_step; - st_prep_data->step_events_remaining -= prep_segment->n_step; - - // Check for end of planner block - if ( st_prep_data->step_events_remaining == 0 ) { - // Set EOB bitflag so stepper algorithm discards the planner block after this segment completes. - prep_segment->flag |= SEGMENT_END_OF_BLOCK; - // Move planner pointer to next block and flag to load a new block for the next segment. - pl_prep_index = plan_next_block_index(pl_prep_index); - pl_prep_block = NULL; - } - - // New step segment completed. Increment segment buffer indices. - segment_buffer_head = segment_next_head; - if ( ++segment_next_head == SEGMENT_BUFFER_SIZE ) { segment_next_head = 0; } - -// long a = prep_segment->n_step; -// printInteger(a); -// printString(" "); - - } -} - -uint8_t st_get_prep_block_index() -{ -// Returns only the index but doesn't state if the block has been partially executed. How do we simply check for this? - return(pl_prep_index); -} - -void st_fetch_partial_block_parameters(uint8_t block_index, float *millimeters_remaining, uint8_t *is_decelerating) -{ - // if called, can we assume that this always changes and needs to be updated? if so, then - // we can perform all of the segment buffer setup tasks here to make sure the next time - // the segments are loaded, the st_data buffer is updated correctly. - // !!! Make sure that this is always pointing to the correct st_prep_data block. - - // When a mid-block acceleration occurs, we have to make sure the ramp counters are updated - // correctly, much in the same fashion as the deceleration counters. Need to think about this - // make sure this is right, but i'm pretty sure it is. - - // TODO: NULL means that the segment buffer has just completed a planner block. Clean up! - if (pl_prep_block != NULL) { - *millimeters_remaining = st_prep_data->step_events_remaining*st_prep_data->mm_per_step; - if (st_prep_data->decelerate_after > 0) { *is_decelerating = false; } - else { *is_decelerating = true; } - - // Flag for new prep_block when st_prep_buffer() is called after the planner recomputes. - pl_partial_block_flag = true; - pl_prep_block = NULL; - } - return; -} diff --git a/archive/stepper_time.c b/archive/stepper_time.c deleted file mode 100644 index 4b73098..0000000 --- a/archive/stepper_time.c +++ /dev/null @@ -1,775 +0,0 @@ -/* - stepper.c - stepper motor driver: executes motion plans using stepper motors - Part of Grbl - - Copyright (c) 2011-2013 Sungeun K. Jeon - Copyright (c) 2009-2011 Simen Svale Skogsrud - - 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 - (at your option) any later version. - - Grbl is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with Grbl. If not, see . -*/ - -#include -#include "stepper.h" -#include "config.h" -#include "settings.h" -#include "planner.h" -#include "nuts_bolts.h" - -// Some useful constants -#define TICKS_PER_MICROSECOND (F_CPU/1000000) - -#define RAMP_NOOP_CRUISE 0 -#define RAMP_ACCEL 1 -#define RAMP_DECEL 2 - -#define LOAD_NOOP 0 -#define LOAD_SEGMENT 1 -#define LOAD_BLOCK 2 - -#define SEGMENT_NOOP 0 -#define SEGMENT_END_OF_BLOCK bit(0) -#define RAMP_CHANGE_ACCEL bit(1) -#define RAMP_CHANGE_DECEL bit(2) - -#define MINIMUM_STEPS_PER_SEGMENT 1 // Don't change - -#define SEGMENT_BUFFER_SIZE 6 - -#define DT_SEGMENT ACCELERATION_TICKS_PER_SECOND/ISR_TICKS_PER_SECOND - -// Stepper state variable. Contains running data and trapezoid variables. -typedef struct { - // Used by the bresenham line algorithm - int32_t counter_x, // Counter variables for the bresenham line tracer - counter_y, - counter_z; - - // Used by inverse time algorithm to track step rate - int32_t counter_dist; // Inverse time distance traveled since last step event - - uint8_t step_count; // Steps remaining in line segment motion - uint8_t phase_count; // Phase ticks remaining after line segment steps complete - - // Used by the stepper driver interrupt - uint8_t execute_step; // Flags step execution for each interrupt. - uint8_t step_pulse_time; // Step pulse reset time after step rise - uint8_t out_bits; // The next stepping-bits to be output - uint8_t load_flag; -} stepper_t; -static stepper_t st; - -// Stores stepper common data for executing steps in the segment buffer. Data can change mid-block when the -// planner updates the remaining block velocity profile with a more optimal plan or a feedrate override occurs. -// NOTE: Normally, this buffer is partially in-use, but, for the worst case scenario, it will never exceed -// the number of accessible stepper buffer segments (SEGMENT_BUFFER_SIZE-1). -typedef struct { - uint32_t dist_per_step; - float step_events_remaining; // Tracks step event count for the executing planner block - float accelerate_until; - float decelerate_after; - float current_rate; - float maximum_rate; - float exit_rate; - - float acceleration; - float step_per_mm; -} st_data_t; -static st_data_t segment_data[SEGMENT_BUFFER_SIZE-1]; - -// Primary stepper segment ring buffer. Contains small, short line segments for the stepper algorithm to execute, -// which are "checked-out" incrementally from the first block in the planner buffer. Once "checked-out", the steps -// in the segments buffer cannot be modified by the planner, where the remaining planner block steps still can. -typedef struct { - uint8_t n_step; // Number of step events to be executed for this segment - uint8_t n_phase_tick; - uint32_t dist_per_tick; - uint8_t st_data_index; // Stepper buffer common data index. Uses this information to execute this segment. - uint8_t flag; // Stepper algorithm bit-flag for special execution conditions. -} st_segment_t; -static st_segment_t segment_buffer[SEGMENT_BUFFER_SIZE]; - -// Step segment ring buffer indices -static volatile uint8_t segment_buffer_tail; -static volatile uint8_t segment_buffer_head; -static uint8_t segment_next_head; - -static volatile uint8_t busy; // Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though. -static plan_block_t *pl_current_block; // A pointer to the planner block currently being traced -static st_segment_t *st_current_segment; -static st_data_t *st_current_data; - -// Pointers for the step segment being prepped from the planner buffer. Accessed only by the -// main program. Pointers may be planning segments or planner blocks ahead of what being executed. -static plan_block_t *pl_prep_block; // Pointer to the planner block being prepped -static st_data_t *st_prep_data; // Pointer to the stepper common data being prepped -static uint8_t pl_prep_index; // Index of planner block being prepped -static uint8_t st_data_prep_index; // Index of stepper common data block being prepped -static uint8_t pl_partial_block_flag; // Flag indicating the planner has modified the prepped planner block - - -/* __________________________ - /| |\ _________________ ^ - / | | \ /| |\ | - / | | \ / | | \ s - / | | | | | \ p - / | | | | | \ e - +-----+------------------------+---+--+---------------+----+ e - | BLOCK 1 | BLOCK 2 | d - - time -----> - - The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta - until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after - after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as - +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. -*/ - -// Stepper state initialization. Cycle should only start if the st.cycle_start flag is -// enabled. Startup init and limits call this function but shouldn't start the cycle. -void st_wake_up() -{ - // Enable steppers by resetting the stepper disable port - if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { - STEPPERS_DISABLE_PORT |= (1<> 3); - // Enable stepper driver interrupt - st.execute_step = false; - st.load_flag = LOAD_BLOCK; - - TCNT2 = 0; // Clear Timer2 - TIMSK2 |= (1<n_step; - - // If the new segment starts a new planner block, initialize stepper variables and counters. - // NOTE: For new segments only, the step counters are not updated to ensure step phasing is continuous. - if (st.load_flag == LOAD_BLOCK) { - pl_current_block = plan_get_current_block(); // Should always be there. Stepper buffer handles this. - st_current_data = &segment_data[segment_buffer[segment_buffer_tail].st_data_index]; - - // Initialize direction bits for block. Set execute flag to set directions bits upon next ISR tick. - st.out_bits = pl_current_block->direction_bits ^ settings.invert_mask; - st.execute_step = true; - - // Initialize Bresenham line counters - st.counter_x = (pl_current_block->step_event_count >> 1); - st.counter_y = st.counter_x; - st.counter_z = st.counter_x; - - // Initialize inverse time, step rate data, and acceleration ramp counters - st.counter_dist = st_current_data->dist_per_step; // dist_per_step always greater than dist_per_tick. - } - - st.load_flag = LOAD_NOOP; // Segment motion loaded. Set no-operation flag to skip during execution. - - } else { - // Can't discard planner block here if a feed hold stops in middle of block. - st_go_idle(); - bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end - return; // Nothing to do but exit. - } - - } - - // Iterate inverse time counter. Triggers each Bresenham step event. - st.counter_dist -= st_current_segment->dist_per_tick; - - // Execute Bresenham step event, when it's time to do so. - if (st.counter_dist < 0) { - if (st.step_count > 0) { // Block phase correction from executing step. - st.counter_dist += st_current_data->dist_per_step; // Reload inverse time counter - - st.out_bits = pl_current_block->direction_bits; // Reset out_bits and reload direction bits - st.execute_step = true; - - // Execute step displacement profile by Bresenham line algorithm - st.counter_x -= pl_current_block->steps[X_AXIS]; - if (st.counter_x < 0) { - st.out_bits |= (1<step_event_count; - if (st.out_bits & (1<steps[Y_AXIS]; - if (st.counter_y < 0) { - st.out_bits |= (1<step_event_count; - if (st.out_bits & (1<steps[Z_AXIS]; - if (st.counter_z < 0) { - st.out_bits |= (1<step_event_count; - if (st.out_bits & (1<flag & SEGMENT_END_OF_BLOCK) { - plan_discard_current_block(); - st.load_flag = LOAD_BLOCK; - } else { - st.load_flag = LOAD_SEGMENT; - } - - // Discard current segment by advancing buffer tail index - if ( ++segment_buffer_tail == SEGMENT_BUFFER_SIZE) { segment_buffer_tail = 0; } - } - st.phase_count--; - } - - - busy = false; -// SPINDLE_ENABLE_PORT ^= 1<step_events_remaining); -// st.ramp_type = RAMP_ACCEL; -// st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK/2; -// st.ramp_rate = 0; -// sys.state = STATE_QUEUED; -// } else { -// sys.state = STATE_IDLE; -// } - sys.state = STATE_IDLE; - -} - - -/* Prepares step segment buffer. Continuously called from main program. - - The segment buffer is an intermediary buffer interface between the execution of steps - by the stepper algorithm and the velocity profiles generated by the planner. The stepper - algorithm only executes steps within the segment buffer and is filled by the main program - when steps are "checked-out" from the first block in the planner buffer. This keeps the - step execution and planning optimization processes atomic and protected from each other. - The number of steps "checked-out" from the planner buffer and the number of segments in - the segment buffer is sized and computed such that no operation in the main program takes - longer than the time it takes the stepper algorithm to empty it before refilling it. - Currently, the segment buffer conservatively holds roughly up to 40-60 msec of steps. - - NOTE: The segment buffer executes a set number of steps over an approximate time period. - If we try to execute over a fixed time period, it is difficult to guarantee or predict - how many steps will execute over it, especially when the step pulse phasing between the - neighboring segments must also be kept consistent. Meaning that, if the last segment step - pulses right before a segment end, the next segment must delay its first pulse so that the - step pulses are consistently spaced apart over time to keep the step pulse train nice and - smooth. Keeping track of phasing and ensuring that the exact number of steps are executed - as defined by the planner block, the related computational overhead can get quickly and - prohibitively expensive, especially in real-time. - Since the stepper algorithm automatically takes care of the step pulse phasing with - its ramp and inverse time counters by retaining the count remainders, we don't have to - explicitly and expensively track and synchronize the exact number of steps, time, and - phasing of steps. All we need to do is approximate the number of steps in each segment - such that the segment buffer has enough execution time for the main program to do what - it needs to do and refill it when it comes back. In other words, we just need to compute - a cheap approximation of the current velocity and the number of steps over it. -*/ - -/* - TODO: Figure out how to enforce a deceleration when a feedrate override is reduced. - The problem is that when an override is reduced, the planner may not plan back to - the current rate. Meaning that the velocity profiles for certain conditions no longer - are trapezoidal or triangular. For example, if the current block is cruising at a - nominal rate and the feedrate override is reduced, the new nominal rate will now be - lower. The velocity profile must first decelerate to the new nominal rate and then - follow on the new plan. So the remaining velocity profile will have a decelerate, - cruise, and another decelerate. - Another issue is whether or not a feedrate override reduction causes a deceleration - that acts over several planner blocks. For example, say that the plan is already - heavily decelerating throughout it, reducing the feedrate will not do much to it. So, - how do we determine when to resume the new plan? How many blocks do we have to wait - until the new plan intersects with the deceleration curve? One plus though, the - deceleration will never be more than the number of blocks in the entire planner buffer, - but it theoretically can be equal to it when all planner blocks are decelerating already. -*/ -void st_prep_buffer() -{ - if (sys.state == STATE_QUEUED) { return; } // Block until a motion state is issued - while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer. - - // Initialize new segment - st_segment_t *prep_segment = &segment_buffer[segment_buffer_head]; - prep_segment->flag = SEGMENT_NOOP; - - // ----------------------------------------------------------------------------------- - // Determine if we need to load a new planner block. If so, prepare step data. - if (pl_prep_block == NULL) { - pl_prep_block = plan_get_block_by_index(pl_prep_index); // Query planner for a queued block - if (pl_prep_block == NULL) { return; } // No planner blocks. Exit. - SPINDLE_ENABLE_PORT ^= 1<step_events_remaining = last_st_prep_data->step_events_remaining; - st_prep_data->dist_per_step = last_st_prep_data->dist_per_step; - st_prep_data->step_per_mm = last_st_prep_data->step_per_mm; - st_prep_data->acceleration = last_st_prep_data->acceleration; - - pl_partial_block_flag = false; // Reset flag - - } else { - - // Prepare commonly shared planner block data for the ensuing segment buffer moves ad-hoc, since - // the planner buffer can dynamically change the velocity profile data as blocks are added. - st_prep_data = &segment_data[st_data_prep_index]; - - // Initialize planner block step data - st_prep_data->step_events_remaining = pl_prep_block->step_event_count; - st_prep_data->step_per_mm = pl_prep_block->step_event_count/pl_prep_block->millimeters; - st_prep_data->dist_per_step = ceil(INV_TIME_MULTIPLIER/st_prep_data->step_per_mm); // (mult*mm/step) - st_prep_data->acceleration = st_prep_data->step_per_mm*pl_prep_block->acceleration; - - } - - // Convert planner entry speed to stepper initial rate. - st_prep_data->current_rate = st_prep_data->step_per_mm*sqrt(pl_prep_block->entry_speed_sqr); - - // Determine current block exit speed - plan_block_t *pl_next_block = plan_get_block_by_index(plan_next_block_index(pl_prep_index)); - float exit_speed_sqr; - if (pl_next_block != NULL) { - exit_speed_sqr = pl_next_block->entry_speed_sqr; - st_prep_data->exit_rate = st_prep_data->step_per_mm*sqrt(exit_speed_sqr); - } else { - exit_speed_sqr = 0.0; // End of planner buffer. Zero speed. - st_prep_data->exit_rate = 0.0; - } - - // Determine velocity profile based on the 7 possible types: Cruise-only, cruise-deceleration, - // acceleration-cruise, acceleration-only, deceleration-only, trapezoid, and triangle. - st_prep_data->accelerate_until = pl_prep_block->millimeters; - if (pl_prep_block->entry_speed_sqr == pl_prep_block->nominal_speed_sqr) { - st_prep_data->maximum_rate = sqrt(pl_prep_block->nominal_speed_sqr); - st_prep_data->accelerate_until = pl_prep_block->millimeters; - if (exit_speed_sqr == pl_prep_block->nominal_speed_sqr) { // Cruise-only type - st_prep_data->decelerate_after = 0.0; - } else { // Cruise-deceleration type - st_prep_data->decelerate_after = (pl_prep_block->nominal_speed_sqr-exit_speed_sqr)/(2*pl_prep_block->acceleration); - } - } else if (exit_speed_sqr == pl_prep_block->nominal_speed_sqr) { - // Acceleration-cruise type - st_prep_data->maximum_rate = sqrt(pl_prep_block->nominal_speed_sqr); - st_prep_data->decelerate_after = 0.0; - st_prep_data->accelerate_until -= (pl_prep_block->nominal_speed_sqr-pl_prep_block->entry_speed_sqr)/(2*pl_prep_block->acceleration); - } else { - float intersection_dist = 0.5*( pl_prep_block->millimeters + (pl_prep_block->entry_speed_sqr - - exit_speed_sqr)/(2*pl_prep_block->acceleration) ); - if (intersection_dist > 0.0) { - if (intersection_dist < pl_prep_block->millimeters) { // Either trapezoid or triangle types - st_prep_data->decelerate_after = (pl_prep_block->nominal_speed_sqr-exit_speed_sqr)/(2*pl_prep_block->acceleration); - if (st_prep_data->decelerate_after < intersection_dist) { // Trapezoid type - st_prep_data->maximum_rate = sqrt(pl_prep_block->nominal_speed_sqr); - st_prep_data->accelerate_until -= (pl_prep_block->nominal_speed_sqr-pl_prep_block->entry_speed_sqr)/(2*pl_prep_block->acceleration); - } else { // Triangle type - st_prep_data->decelerate_after = intersection_dist; - st_prep_data->maximum_rate = sqrt(2*pl_prep_block->acceleration*st_prep_data->decelerate_after+exit_speed_sqr); - st_prep_data->accelerate_until -= st_prep_data->decelerate_after; - } - } else { // Deceleration-only type - st_prep_data->maximum_rate = sqrt(pl_prep_block->entry_speed_sqr); - st_prep_data->decelerate_after = pl_prep_block->millimeters; - } - } else { // Acceleration-only type - st_prep_data->maximum_rate = sqrt(exit_speed_sqr); - st_prep_data->decelerate_after = 0.0; - st_prep_data->accelerate_until = 0.0; - } - } - - // Determine block velocity profile parameters -// st_prep_data->accelerate_until = (pl_prep_block->nominal_speed_sqr-pl_prep_block->entry_speed_sqr)/(2*pl_prep_block->acceleration); -// st_prep_data->decelerate_after = (pl_prep_block->nominal_speed_sqr-exit_speed_sqr)/(2*pl_prep_block->acceleration); -// -// // Determine if velocity profile is a triangle or trapezoid. -// if (pl_prep_block->millimeters < st_prep_data->accelerate_until+st_prep_data->decelerate_after) { -// st_prep_data->decelerate_after = 0.5*( pl_prep_block->millimeters + (pl_prep_block->entry_speed_sqr -// - exit_speed_sqr)/(2*pl_prep_block->acceleration) ); -// st_prep_data->accelerate_until = pl_prep_block->millimeters-st_prep_data->decelerate_after; -// st_prep_data->maximum_speed = sqrt(2*pl_prep_block->acceleration*st_prep_data->decelerate_after+exit_speed_sqr); -// } else { -// st_prep_data->accelerate_until = pl_prep_block->millimeters-st_prep_data->accelerate_until; -// st_prep_data->maximum_speed = sqrt(pl_prep_block->nominal_speed_sqr); -// } - - // Convert velocity profile parameters in terms of steps. - st_prep_data->maximum_rate *= st_prep_data->step_per_mm; - st_prep_data->accelerate_until *= st_prep_data->step_per_mm; - st_prep_data->decelerate_after *= st_prep_data->step_per_mm; - - } - - // Set new segment to point to the current segment data block. - prep_segment->st_data_index = st_data_prep_index; - - // ----------------------------------------------------------------------------------- - // Initialize segment execute distance. Attempt to create a full segment over DT_SEGMENT. - // NOTE: Computed in terms of steps and seconds to prevent numerical round-off issues. - - float steps_remaining = st_prep_data->step_events_remaining; - float dt = DT_SEGMENT; - if (steps_remaining > st_prep_data->accelerate_until) { // Acceleration ramp - steps_remaining -= st_prep_data->current_rate*DT_SEGMENT - + st_prep_data->acceleration*(0.5*DT_SEGMENT*DT_SEGMENT); - if (steps_remaining < st_prep_data->accelerate_until) { // **Incomplete** Acceleration ramp end. - // Acceleration-cruise, acceleration-deceleration ramp junction, or end of block - steps_remaining = st_prep_data->accelerate_until; - dt = 2*(st_prep_data->step_events_remaining-steps_remaining)/ - (st_prep_data->current_rate+st_prep_data->maximum_rate); - st_prep_data->current_rate = st_prep_data->maximum_rate; - } else { // **Complete** Acceleration only. - st_prep_data->current_rate += st_prep_data->acceleration*DT_SEGMENT; - } - } else if (steps_remaining <= st_prep_data->decelerate_after) { // Deceleration ramp - steps_remaining -= st_prep_data->current_rate*DT_SEGMENT - - st_prep_data->acceleration*(0.5*DT_SEGMENT*DT_SEGMENT); - if (steps_remaining > 0) { // **Complete** Deceleration only. - st_prep_data->current_rate -= st_prep_data->acceleration*DT_SEGMENT; - } else { // **Complete* End of block. - dt = 2*st_prep_data->step_events_remaining/(st_prep_data->current_rate+st_prep_data->exit_rate); - steps_remaining = 0; - // st_prep_data->current_speed = st_prep_data->exit_speed; - } - } else { // Cruising profile - steps_remaining -= st_prep_data->maximum_rate*DT_SEGMENT; - if (steps_remaining < st_prep_data->decelerate_after) { // **Incomplete** End of cruise. - steps_remaining = st_prep_data->decelerate_after; - dt = (st_prep_data->step_events_remaining-steps_remaining)/st_prep_data->maximum_rate; - } // Otherwise **Complete** Cruising only. - } - - // ----------------------------------------------------------------------------------- - // If segment is incomplete, attempt to fill the remainder. - // NOTE: Segment remainder always spans a cruise and/or a deceleration ramp. - - if (dt < DT_SEGMENT) { - if (steps_remaining > 0) { // Skip if end of block. - float last_steps_remaining; - - // Fill incomplete segment with an acceleration junction. - if (steps_remaining > st_prep_data->decelerate_after) { // Cruising profile - last_steps_remaining = steps_remaining; - steps_remaining -= st_prep_data->current_rate*(DT_SEGMENT-dt); - if (steps_remaining < st_prep_data->decelerate_after) { // **Incomplete** - steps_remaining = st_prep_data->decelerate_after; - dt += (last_steps_remaining-steps_remaining)/st_prep_data->maximum_rate; - // current_speed = maximum_speed; - } else { // **Complete** Segment filled. - dt = DT_SEGMENT; - } - } - - // Fill incomplete segment with a deceleration junction. - if (steps_remaining > 0) { - if (steps_remaining <= st_prep_data->decelerate_after) { // Deceleration ramp - last_steps_remaining = steps_remaining; - float dt_remainder = DT_SEGMENT-dt; - steps_remaining -= dt_remainder*(st_prep_data->current_rate - - 0.5*st_prep_data->acceleration*dt_remainder); - if (steps_remaining > 0) { // **Complete** Segment filled. - st_prep_data->current_rate -= st_prep_data->acceleration*dt_remainder; - dt = DT_SEGMENT; - } else { // **Complete** End of block. - steps_remaining = 0; - dt += (2*last_steps_remaining/(st_prep_data->current_rate+st_prep_data->exit_rate)); - // st_prep_data->current_speed = st_prep_data->exit_speed; - } - } - } - - } - } - - // ----------------------------------------------------------------------------------- - // Compute segment step rate, steps to execute, and step phase correction parameters. - // NOTE: - - // !!! PROBLEM. Step events remaining in floating point can limit the number of steps - // we can accurately track, since floats have 8 significant digits. However, this only - // becomes a problem if there are more than 10,000,000, which translates to a CNC machine - // with 800 step/mm and 10 meters of axis travel. - - prep_segment->dist_per_tick = ceil((st_prep_data->step_events_remaining-steps_remaining) - /dt*(INV_TIME_MULTIPLIER/ISR_TICKS_PER_SECOND)); // (mult*mm/isr_tic) - - if (steps_remaining > 0) { - - // Compute number of steps to execute and segment step phase correction. - prep_segment->n_step = ceil(st_prep_data->step_events_remaining)-ceil(steps_remaining); - prep_segment->n_phase_tick = ceil((ceil(steps_remaining)-steps_remaining)*st_prep_data->dist_per_step); - - } else { // End of block. Finish it out. - - // Set to execute the remaining steps and no phase correction upon finishing the block. - prep_segment->n_step = ceil(st_prep_data->step_events_remaining); - prep_segment->n_phase_tick = 0; - - // Move planner pointer to next block and flag to load a new block for the next segment. - pl_prep_index = plan_next_block_index(pl_prep_index); - pl_prep_block = NULL; - prep_segment->flag |= SEGMENT_END_OF_BLOCK; - } - - // Update step execution variables - st_prep_data->step_events_remaining = steps_remaining; - - // Ensure the initial step rate exceeds the MINIMUM_STEP_RATE. - // TODO: Use config.h error checking to do this. Otherwise, counters get screwy. - - // New step segment initialization completed. Increment segment buffer indices. - segment_buffer_head = segment_next_head; - if ( ++segment_next_head == SEGMENT_BUFFER_SIZE ) { segment_next_head = 0; } - SPINDLE_ENABLE_PORT ^= 1<step_events_remaining/st_prep_data->step_per_mm; - if (st_prep_data->step_events_remaining < st_prep_data->decelerate_after) { *is_decelerating = true; } - else { *is_decelerating = false; } - - // Flag for new prep_block when st_prep_buffer() is called after the planner recomputes. - pl_partial_block_flag = true; - pl_prep_block = NULL; - } - return; -} diff --git a/archive/stepper_time_archive.c b/archive/stepper_time_archive.c deleted file mode 100644 index dac918f..0000000 --- a/archive/stepper_time_archive.c +++ /dev/null @@ -1,823 +0,0 @@ -/* - stepper.c - stepper motor driver: executes motion plans using stepper motors - Part of Grbl - - Copyright (c) 2011-2013 Sungeun K. Jeon - Copyright (c) 2009-2011 Simen Svale Skogsrud - - 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 - (at your option) any later version. - - Grbl is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with Grbl. If not, see . -*/ - -#include -#include "stepper.h" -#include "config.h" -#include "settings.h" -#include "planner.h" -#include "nuts_bolts.h" - -// Some useful constants -#define TICKS_PER_MICROSECOND (F_CPU/1000000) - -#define RAMP_ACCEL 0 -#define RAMP_CRUISE 1 -#define RAMP_DECEL 2 - -#define LOAD_NOOP 0 -#define LOAD_SEGMENT 1 -#define LOAD_BLOCK 2 - -#define SEGMENT_NOOP 0 -#define SEGMENT_END_OF_BLOCK bit(0) -#define RAMP_CHANGE_ACCEL bit(1) -#define RAMP_CHANGE_DECEL bit(2) - -#define SEGMENT_BUFFER_SIZE 6 - -#define DT_SEGMENT (1.0/(ACCELERATION_TICKS_PER_SECOND*60.0)) - -// Stores the planner block Bresenham algorithm execution data for the segments in the segment -// buffer. Normally, this buffer is partially in-use, but, for the worst case scenario, it will -// never exceed the number of accessible stepper buffer segments (SEGMENT_BUFFER_SIZE-1). -// NOTE: This data is copied from the prepped planner blocks so that the planner blocks may be -// discarded when entirely consumed and completed by the segment buffer. -typedef struct { - uint8_t direction_bits; - int32_t steps[N_AXIS]; - int32_t step_event_count; -} st_block_t; -static st_block_t st_block_buffer[SEGMENT_BUFFER_SIZE-1]; -// TODO: Directly adjust this parameters to stop motion of individual axes for the homing cycle. -// But this may require this to be volatile if it is controlled by an interrupt. - -// Primary stepper segment ring buffer. Contains small, short line segments for the stepper -// algorithm to execute, which are "checked-out" incrementally from the first block in the -// planner buffer. Once "checked-out", the steps in the segments buffer cannot be modified by -// the planner, where the remaining planner block steps still can. -typedef struct { - uint8_t n_step; // Number of step events to be executed for this segment - uint8_t st_block_index; // Stepper block data index. Uses this information to execute this segment. - int32_t phase_dist; - int32_t dist_per_tick; -} segment_t; -static segment_t segment_buffer[SEGMENT_BUFFER_SIZE]; - -// Stepper state variable. Contains running data and trapezoid variables. -typedef struct { - // Used by the bresenham line algorithm - int32_t counter_x, // Counter variables for the bresenham line tracer - counter_y, - counter_z; - - // Used by inverse time algorithm to track step rate - int32_t counter_dist; // Inverse time distance traveled since last step event - - // Used by the stepper driver interrupt - uint8_t execute_step; // Flags step execution for each interrupt. - uint8_t step_pulse_time; // Step pulse reset time after step rise - uint8_t out_bits; // The next stepping-bits to be output - - uint8_t step_count; // Steps remaining in line segment motion - uint8_t exec_block_index; // Tracks the current st_block index. Change indicates new block. - st_block_t *exec_block; // Pointer to the block data for the segment being executed - segment_t *exec_segment; // Pointer to the segment being executed -} stepper_t; -static stepper_t st; - -// Step segment ring buffer indices -static volatile uint8_t segment_buffer_tail; -static volatile uint8_t segment_buffer_head; -static uint8_t segment_next_head; - -static volatile uint8_t busy; // Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though. - -// Pointers for the step segment being prepped from the planner buffer. Accessed only by the -// main program. Pointers may be planning segments or planner blocks ahead of what being executed. -static plan_block_t *pl_block; // Pointer to the planner block being prepped -static st_block_t *st_prep_block; // Pointer to the stepper block data being prepped - -typedef struct { - uint8_t st_block_index; // Index of stepper common data block being prepped - uint8_t partial_block_flag; // Flag indicating the planner has modified the prepped planner block - - float step_per_mm; - float step_events_remaining; // Tracks step event count for the executing planner block -// int32_t step_events_remaining; - float step_remainder; - - uint8_t ramp_type; - float current_speed; - float maximum_speed; - float exit_speed; - float accelerate_until; - float decelerate_after; -} st_prep_t; -static st_prep_t prep; - - -/* __________________________ - /| |\ _________________ ^ - / | | \ /| |\ | - / | | \ / | | \ s - / | | | | | \ p - / | | | | | \ e - +-----+------------------------+---+--+---------------+----+ e - | BLOCK 1 | BLOCK 2 | d - - time -----> - - The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta - until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after - after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as - +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. -*/ - -// Stepper state initialization. Cycle should only start if the st.cycle_start flag is -// enabled. Startup init and limits call this function but shouldn't start the cycle. -void st_wake_up() -{ - // Enable steppers by resetting the stepper disable port - if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { - STEPPERS_DISABLE_PORT |= (1<> 3); - - // Enable stepper driver interrupt - TCNT2 = 0; // Clear Timer2 - TIMSK2 |= (1<n_step; - - // If the new segment starts a new planner block, initialize stepper variables and counters. - // NOTE: When the segment data index changes, this indicates a new planner block. - if ( st.exec_block_index != st.exec_segment->st_block_index ) { - st.exec_block_index = st.exec_segment->st_block_index; - st.exec_block = &st_block_buffer[st.exec_block_index]; - - // Initialize direction bits for block. Set execute flag to set directions bits upon next ISR tick. - st.out_bits = st.exec_block->direction_bits ^ settings.invert_mask; - st.execute_step = true; - - // Initialize Bresenham line counters - st.counter_x = (st.exec_block->step_event_count >> 1); - st.counter_y = st.counter_x; - st.counter_z = st.counter_x; - - // Initialize inverse time, step rate data, and acceleration ramp counters - st.counter_dist = INV_TIME_MULTIPLIER; // dist_per_step always greater than dist_per_tick. - } - - } else { - // Segment buffer empty. Shutdown. - st_go_idle(); - bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end - return; // Nothing to do but exit. - } - - } - - // Iterate inverse time counter. Triggers each Bresenham step event. - st.counter_dist -= st.exec_segment->dist_per_tick; - - // Execute Bresenham step event, when it's time to do so. - if (st.counter_dist < 0) { - if (st.step_count != 0) { // Block phase correction from executing step. - st.counter_dist += INV_TIME_MULTIPLIER; // Reload inverse time counter - st.step_count--; // Decrement step events count - - // Execute step displacement profile by Bresenham line algorithm - st.execute_step = true; - st.out_bits = st.exec_block->direction_bits; // Reset out_bits and reload direction bits - st.counter_x -= st.exec_block->steps[X_AXIS]; - if (st.counter_x < 0) { - st.out_bits |= (1<step_event_count; - if (st.out_bits & (1<steps[Y_AXIS]; - if (st.counter_y < 0) { - st.out_bits |= (1<step_event_count; - if (st.out_bits & (1<steps[Z_AXIS]; - if (st.counter_z < 0) { - st.out_bits |= (1<step_event_count; - if (st.out_bits & (1<phase_dist > st.counter_dist) { - // Segment is complete. Discard current segment and advance segment indexing. - st.exec_segment = NULL; - if ( ++segment_buffer_tail == SEGMENT_BUFFER_SIZE) { segment_buffer_tail = 0; } - } - } - - busy = false; -// SPINDLE_ENABLE_PORT ^= 1<step_events_remaining); -// st.ramp_type = RAMP_ACCEL; -// st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK/2; -// st.ramp_rate = 0; -// sys.state = STATE_QUEUED; -// } else { -// sys.state = STATE_IDLE; -// } - sys.state = STATE_IDLE; - -} - - -/* Prepares step segment buffer. Continuously called from main program. - - The segment buffer is an intermediary buffer interface between the execution of steps - by the stepper algorithm and the velocity profiles generated by the planner. The stepper - algorithm only executes steps within the segment buffer and is filled by the main program - when steps are "checked-out" from the first block in the planner buffer. This keeps the - step execution and planning optimization processes atomic and protected from each other. - The number of steps "checked-out" from the planner buffer and the number of segments in - the segment buffer is sized and computed such that no operation in the main program takes - longer than the time it takes the stepper algorithm to empty it before refilling it. - Currently, the segment buffer conservatively holds roughly up to 40-60 msec of steps. - - NOTE: The segment buffer executes a set number of steps over an approximate time period. - If we try to execute over a fixed time period, it is difficult to guarantee or predict - how many steps will execute over it, especially when the step pulse phasing between the - neighboring segments must also be kept consistent. Meaning that, if the last segment step - pulses right before a segment end, the next segment must delay its first pulse so that the - step pulses are consistently spaced apart over time to keep the step pulse train nice and - smooth. Keeping track of phasing and ensuring that the exact number of steps are executed - as defined by the planner block, the related computational overhead can get quickly and - prohibitively expensive, especially in real-time. - Since the stepper algorithm automatically takes care of the step pulse phasing with - its ramp and inverse time counters by retaining the count remainders, we don't have to - explicitly and expensively track and synchronize the exact number of steps, time, and - phasing of steps. All we need to do is approximate the number of steps in each segment - such that the segment buffer has enough execution time for the main program to do what - it needs to do and refill it when it comes back. In other words, we just need to compute - a cheap approximation of the current velocity and the number of steps over it. -*/ - -/* - TODO: Figure out how to enforce a deceleration when a feedrate override is reduced. - The problem is that when an override is reduced, the planner may not plan back to - the current rate. Meaning that the velocity profiles for certain conditions no longer - are trapezoidal or triangular. For example, if the current block is cruising at a - nominal rate and the feedrate override is reduced, the new nominal rate will now be - lower. The velocity profile must first decelerate to the new nominal rate and then - follow on the new plan. So the remaining velocity profile will have a decelerate, - cruise, and another decelerate. - Another issue is whether or not a feedrate override reduction causes a deceleration - that acts over several planner blocks. For example, say that the plan is already - heavily decelerating throughout it, reducing the feedrate will not do much to it. So, - how do we determine when to resume the new plan? How many blocks do we have to wait - until the new plan intersects with the deceleration curve? One plus though, the - deceleration will never be more than the number of blocks in the entire planner buffer, - but it theoretically can be equal to it when all planner blocks are decelerating already. -*/ -void st_prep_buffer() -{ - if (sys.state == STATE_QUEUED) { return; } // Block until a motion state is issued - while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer. - - - // ----------------------------------------------------------------------------------- - // Determine if we need to load a new planner block. If so, prepare step data. - if (pl_block == NULL) { - pl_block = plan_get_current_block(); // Query planner for a queued block - if (pl_block == NULL) { return; } // No planner blocks. Exit. - -// SPINDLE_ENABLE_PORT ^= 1<steps[X_AXIS] = pl_block->steps[X_AXIS]; - st_prep_block->steps[Y_AXIS] = pl_block->steps[Y_AXIS]; - st_prep_block->steps[Z_AXIS] = pl_block->steps[Z_AXIS]; - st_prep_block->direction_bits = pl_block->direction_bits; - st_prep_block->step_event_count = pl_block->step_event_count; - - // Initialize planner block step count, unit distance data, and remainder tracker. - prep.step_per_mm = ((float)st_prep_block->step_event_count)/pl_block->millimeters; - prep.step_events_remaining = st_prep_block->step_event_count; - prep.step_remainder = 0.0; - } - - // Compute the prepped planner block velocity profile to be traced by stepper algorithm. - prep.current_speed = sqrt(pl_block->entry_speed_sqr); - prep.exit_speed = plan_get_exec_block_exit_speed(); - - // Determine velocity profile based on the 7 possible types: Cruise-only, cruise-deceleration, - // acceleration-cruise, acceleration-only, deceleration-only, full-trapezoid, and triangle. - prep.ramp_type = RAMP_ACCEL; - float exit_speed_sqr = prep.exit_speed*prep.exit_speed; - float inv_2_accel = 0.5/pl_block->acceleration; - float intersection_dist = - 0.5*(pl_block->millimeters+inv_2_accel*(pl_block->entry_speed_sqr-exit_speed_sqr)); - if (intersection_dist > 0.0) { - if (intersection_dist < pl_block->millimeters) { // Either trapezoid or triangle types - // NOTE: For acceleration-cruise trapezoid, following calculation will be 0.0. - prep.decelerate_after = inv_2_accel*(pl_block->nominal_speed_sqr-exit_speed_sqr); - if (prep.decelerate_after < intersection_dist) { // Trapezoid type - prep.maximum_speed = sqrt(pl_block->nominal_speed_sqr); - if (pl_block->entry_speed_sqr == pl_block->nominal_speed_sqr) { - // Cruise-deceleration or cruise-only type. - prep.ramp_type = RAMP_CRUISE; - prep.accelerate_until = pl_block->millimeters; - } else { - // Full-trapezoid or acceleration-cruise types - prep.accelerate_until = - pl_block->millimeters-inv_2_accel*(pl_block->nominal_speed_sqr-pl_block->entry_speed_sqr); - } - } else { // Triangle type - prep.accelerate_until = intersection_dist; - prep.decelerate_after = intersection_dist; - prep.maximum_speed = sqrt(2.0*pl_block->acceleration*intersection_dist+exit_speed_sqr); - } - } else { // Deceleration-only type - prep.ramp_type = RAMP_DECEL; - prep.maximum_speed = prep.current_speed; - prep.accelerate_until = pl_block->millimeters; - prep.decelerate_after = pl_block->millimeters; - } - } else { // Acceleration-only type - prep.maximum_speed = prep.exit_speed; - prep.accelerate_until = 0.0; - prep.decelerate_after = 0.0; - } - - } - - // Initialize new segment - segment_t *prep_segment = &segment_buffer[segment_buffer_head]; - - // Set new segment to point to the current segment data block. - prep_segment->st_block_index = prep.st_block_index; - - /* ----------------------------------------------------------------------------------- - Compute the average velocity of this new segment by determining the total distance - traveled over the segment time DT_SEGMENT. This section attempts to create a full - segment based on the current ramp conditions. If the segment is incomplete and - terminates upon a ramp change, the next section will attempt to fill the remaining - segment execution time. However, if an incomplete segment terminates at the end of - the planner block, the segment execution time is less than DT_SEGMENT and the new - segment will execute over this truncated execution time. - */ - float dt = 0.0; - float mm_remaining = pl_block->millimeters; - float dt_var = DT_SEGMENT; - float mm_var; - do { - switch (prep.ramp_type) { - case RAMP_ACCEL: - // NOTE: Acceleration ramp always computes during first loop only. - mm_remaining -= DT_SEGMENT*(prep.current_speed + pl_block->acceleration*(0.5*DT_SEGMENT)); - if (mm_remaining < prep.accelerate_until) { // End of acceleration ramp. - // Acceleration-cruise, acceleration-deceleration ramp junction, or end of block. - mm_remaining = prep.accelerate_until; // NOTE: 0.0 at EOB - dt_var = 2.0*(pl_block->millimeters-mm_remaining)/(prep.current_speed+prep.maximum_speed); - if (mm_remaining == prep.decelerate_after) { prep.ramp_type = RAMP_DECEL; } - else { prep.ramp_type = RAMP_CRUISE; } - prep.current_speed = prep.maximum_speed; - } else { // Acceleration only. - prep.current_speed += pl_block->acceleration*dt_var; - } - break; - case RAMP_CRUISE: - // NOTE: mm_var used to retain the last mm_remaining for incomplete segment dt_var calculations. - mm_var = mm_remaining - prep.maximum_speed*dt_var; - if (mm_var < prep.decelerate_after) { // End of cruise. - // Cruise-deceleration junction or end of block. - dt_var = (mm_remaining - prep.decelerate_after)/prep.maximum_speed; - mm_remaining = prep.decelerate_after; // NOTE: 0.0 at EOB - prep.ramp_type = RAMP_DECEL; - } else { // Cruising only. - mm_remaining = mm_var; - } - break; - default: // case RAMP_DECEL: - // NOTE: mm_var used to catch negative decelerate distance values near zero speed. - mm_var = dt_var*(prep.current_speed - 0.5*pl_block->acceleration*dt_var); - if ((mm_var > 0.0) && (mm_var < pl_block->millimeters)) { // Deceleration only. - prep.current_speed -= pl_block->acceleration*dt_var; - // Check for near-zero speed and prevent divide by zero in rare scenarios. - if (prep.current_speed <= prep.exit_speed) { mm_remaining = 0.0; } - else { mm_remaining -= mm_var; } - } else { // End of block. - dt_var = 2.0*mm_remaining/(prep.current_speed+prep.exit_speed); - mm_remaining = 0.0; - // prep.current_speed = prep.exit_speed; - } - } - dt += dt_var; - if (dt < DT_SEGMENT) { dt_var = DT_SEGMENT - dt; } // **Incomplete** At ramp junction. - else { break; } // **Complete** Exit loop. Segment execution time maxed. - } while ( mm_remaining > 0.0 ); // **Complete** Exit loop. End of planner block. - - /* - float mm_remaining; - float dt = DT_SEGMENT; - if (pl_block->millimeters > prep.accelerate_until) { // [Acceleration Ramp] - mm_remaining = pl_block->millimeters - DT_SEGMENT*(prep.current_speed + pl_block->acceleration*(0.5*DT_SEGMENT)); - if (mm_remaining < prep.accelerate_until) { // **Incomplete** Acceleration ramp end. - // Acceleration-cruise, acceleration-deceleration ramp junction, or end of block. - mm_remaining = prep.accelerate_until; // NOTE: 0.0 at EOB - dt = 2.0*(pl_block->millimeters-mm_remaining)/(prep.current_speed+prep.maximum_speed); - prep.current_speed = prep.maximum_speed; - } else { // **Complete** Acceleration only. - prep.current_speed += pl_block->acceleration*DT_SEGMENT; - prep.current_speed = min(prep.maximum_speed,prep.current_speed); - } - } else if (pl_block->millimeters > prep.decelerate_after) { // [No Ramp. Cruising] - mm_remaining = pl_block->millimeters - prep.maximum_speed*DT_SEGMENT; - if (mm_remaining < prep.decelerate_after) { // **Incomplete** End of cruise. - // Cruise-deceleration junction or end of block. - mm_remaining = prep.decelerate_after; // NOTE: 0.0 at EOB - dt = (pl_block->millimeters-mm_remaining)/prep.maximum_speed; - } // Otherwise **Complete** Cruising only. - } else { // [Deceleration Ramp] - mm_remaining = DT_SEGMENT*(prep.current_speed - 0.5*pl_block->acceleration*DT_SEGMENT); - if ((mm_remaining > 0.0) && (mm_remaining < pl_block->millimeters)) { // **Complete** Deceleration only. - prep.current_speed -= pl_block->acceleration*DT_SEGMENT; - if (prep.current_speed <= prep.exit_speed) { // Round off error fix. Prevents divide by zero. - mm_remaining = 0.0; - } else { - mm_remaining = pl_block->millimeters - mm_remaining; - } - } else { // **Complete** End of block. - mm_remaining = 0.0; - dt = 2.0*pl_block->millimeters/(prep.current_speed+prep.exit_speed); - // prep.current_speed = prep.exit_speed; - } - } - - - /* ----------------------------------------------------------------------------------- - If segment is incomplete, attempt to fill the remaining segment execution time. - NOTE: Segment remainder always spans a cruise and/or a deceleration ramp. - - float partial_mm, dt_remainder; - if ((dt < DT_SEGMENT) && (mm_remaining > 0.0)) { - dt_remainder = DT_SEGMENT-dt; - - // Attempt to fill incomplete segment with cruising profile. - if (mm_remaining > prep.decelerate_after) { // Cruising profile - partial_mm = mm_remaining - prep.current_speed*dt_remainder; - if (partial_mm < prep.decelerate_after) { // **Incomplete** - dt += (mm_remaining-prep.decelerate_after)/prep.maximum_speed; - mm_remaining = prep.decelerate_after; - // current_speed = maximum_speed; - } else { // **Complete** Segment filled. - mm_remaining = partial_mm; - dt = DT_SEGMENT; - } - } - - // Attempt to fill incomplete segment with deceleration ramp. - if ((dt < DT_SEGMENT) && (mm_remaining > 0.0)) { - if (mm_remaining <= prep.decelerate_after) { // Deceleration ramp - dt_remainder = DT_SEGMENT-dt; - partial_mm = dt_remainder*(prep.current_speed-0.5*pl_block->acceleration*dt_remainder); - if ((partial_mm > 0.0) && (mm_remaining > partial_mm)) { // **Complete** Segment filled. - prep.current_speed -= pl_block->acceleration*dt_remainder; - if (prep.current_speed <= prep.exit_speed) { - mm_remaining = 0.0; - - } else { - mm_remaining -= partial_mm; - dt = DT_SEGMENT; - } - } else { // **Complete** End of block. - dt += (2.0*mm_remaining/(prep.current_speed+prep.exit_speed)); - mm_remaining = 0.0; - // prep.current_speed = prep.exit_speed; - } - } - } - } - */ -// printString(" Z"); -// printFloat(dt*(60.0*1000.0)); -// printString(" "); -// printFloat(mm_remaining); -// printString(" "); -// printFloat(prep.current_speed); -// printString("Z "); - - /* ----------------------------------------------------------------------------------- - Compute segment step rate, steps to execute, and step phase correction parameters. - */ -// float step_events; -// if (mm_remaining > 0.0) { -// step_events = prep.step_per_mm*(pl_block->millimeters - mm_remaining); // Convert mm to steps -// prep_segment->n_step = floor(step_events + prep.step_remainder); -// if (prep_segment->n_step > prep.step_events_remaining) { // Prevent round-off overshoot -// prep_segment->n_step = prep.step_events_remaining; -// } -// } else { // Ensure all remaining steps are executed -// step_events = prep.step_per_mm*pl_block->millimeters; -// prep_segment->n_step = prep.step_events_remaining; -// } -// prep.step_events_remaining -= prep_segment->n_step; -// -// // Compute segment rate. -// prep_segment->dist_per_tick = -// ceil( (INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND)) * (step_events/dt) ); // (mult*step/isr_tic) -// -// if (prep.step_events_remaining > 0) { -// // Compute step phase distance and update segment continuation parameters. -// prep.step_remainder += step_events - prep_segment->n_step; -// prep_segment->phase_dist = ceil(INV_TIME_MULTIPLIER-INV_TIME_MULTIPLIER*prep.step_remainder); -// pl_block->millimeters = mm_remaining; -// pl_block->entry_speed_sqr = prep.current_speed*prep.current_speed; -// -// } else { // End of block. Finish it out. -// // The planner block is complete. All steps are set to be executed in the segment buffer. -// // Move planner pointer to next block and flag to load a new block for the next segment. -// prep_segment->phase_dist = INV_TIME_MULTIPLIER; -// pl_block = NULL; -// plan_discard_current_block(); -// } - - if (mm_remaining > 0.0) { - - float steps_remaining = prep.step_per_mm*mm_remaining; - prep_segment->dist_per_tick = ceil( (INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))* - ((prep.step_events_remaining-steps_remaining)/dt) ); // (mult*step/isr_tic) - - // Compute number of steps to execute and segment step phase correction. - prep_segment->n_step = ceil(prep.step_events_remaining)-ceil(steps_remaining); - prep_segment->phase_dist = ceil(INV_TIME_MULTIPLIER*(1.0-ceil(steps_remaining)+steps_remaining)); - - // Update step execution variables - prep.step_events_remaining = steps_remaining; - pl_block->millimeters = mm_remaining; - pl_block->entry_speed_sqr = prep.current_speed*prep.current_speed; - - } else { // End of block. Finish it out. - - prep_segment->dist_per_tick = ceil( (INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))* - prep.step_events_remaining/dt ); // (mult*step/isr_tic) - prep_segment->phase_dist = INV_TIME_MULTIPLIER; - - // Set to execute the remaining steps and no phase correction upon finishing the block. - prep_segment->n_step = ceil(prep.step_events_remaining); - - - // NOTE: Not required. Planner will ignore this block as it is now complete. - // prep.step_events_remaining = 0.0; - // pl_block->millimeters = 0.0; - - // The planner block is complete. All steps are set to be executed in the segment buffer. - // Move planner pointer to next block and flag to load a new block for the next segment. - pl_block = NULL; - plan_discard_current_block(); - } - -// long a = prep_segment->n_step; -// printInteger(a); -// printString(" "); -// a = prep_segment->phase_dist; -// printInteger(prep_segment->dist_per_tick); -// printString(" "); -// printFloat(prep.step_events_remaining); -// printString(" "); -// printFloat(pl_block->millimeters); -// printString(" "); - - - // !!! PROBLEM. Step events remaining in floating point can limit the number of steps - // we can accurately track, since floats have ~7.2 significant digits. However, this only - // becomes a problem if there are more than 1,000,000, which translates to a CNC machine - // with 200 step/mm and 5 meters of axis travel. Possible but unlikely. Could have more - // issues with user setting up their machine with too high of steps. - - // TODO: dist_per_tick must be less than INV_TIME_MULTIPLIER. A check can be made to - // make this a hard limit. Need to make sure this doesn't affect the velocity profiles.. - // it shouldn't. The same could said for the minimum allowable step rate too. This should - // not affect the tracing of the profiles either. - - // Ensure the initial step rate exceeds the MINIMUM_STEP_RATE. - // TODO: Use config.h error checking to do this. Otherwise, counters get screwy. - - // New step segment initialization completed. Increment segment buffer indices. - segment_buffer_head = segment_next_head; - if ( ++segment_next_head == SEGMENT_BUFFER_SIZE ) { segment_next_head = 0; } - -int32_t blength = segment_buffer_head - segment_buffer_tail; -if (blength < 0) { blength += SEGMENT_BUFFER_SIZE; } -printInteger(blength); -// SPINDLE_ENABLE_PORT ^= 1<. -*/ - -/* The timer calculations of this module informed by the 'RepRap cartesian firmware' by Zack Smith - and Philipp Tiefenbacher. */ - -#include -#include "stepper.h" -#include "config.h" -#include "settings.h" -#include "planner.h" - -// Some useful constants -#define TICKS_PER_MICROSECOND (F_CPU/1000000) -#define CRUISE_RAMP 0 -#define ACCEL_RAMP 1 -#define DECEL_RAMP 2 - -// Stepper state variable. Contains running data and trapezoid variables. -typedef struct { - // Used by the bresenham line algorithm - int32_t counter_x, // Counter variables for the bresenham line tracer - counter_y, - counter_z; - int32_t event_count; // Total event count. Retained for feed holds. - int32_t step_events_remaining; // Steps remaining in motion - - // Used by Pramod Ranade inverse time algorithm - int32_t delta_d; // Ranade distance traveled per interrupt tick - int32_t d_counter; // Ranade distance traveled since last step event - uint8_t ramp_count; // Acceleration interrupt tick counter. - uint8_t ramp_type; // Ramp type variable. - uint8_t execute_step; // Flags step execution for each interrupt. - -} stepper_t; -static stepper_t st; -static block_t *current_block; // A pointer to the block currently being traced - -// Used by the stepper driver interrupt -static uint8_t step_pulse_time; // Step pulse reset time after step rise -static uint8_t out_bits; // The next stepping-bits to be output - -// NOTE: If the main interrupt is guaranteed to be complete before the next interrupt, then -// this blocking variable is no longer needed. Only here for safety reasons. -static volatile uint8_t busy; // True when "Stepper Driver Interrupt" is being serviced. Used to avoid retriggering that handler. - -// __________________________ -// /| |\ _________________ ^ -// / | | \ /| |\ | -// / | | \ / | | \ s -// / | | | | | \ p -// / | | | | | \ e -// +-----+------------------------+---+--+---------------+----+ e -// | BLOCK 1 | BLOCK 2 | d -// -// time -----> -// -// The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta -// until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after -// after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as -// +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. - - -// Stepper state initialization. Cycle should only start if the st.cycle_start flag is -// enabled. Startup init and limits call this function but shouldn't start the cycle. -void st_wake_up() -{ - // Enable steppers by resetting the stepper disable port - if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { - STEPPERS_DISABLE_PORT |= (1<> 3); - // Enable stepper driver interrupt - st.execute_step = false; - TCNT2 = 0; // Clear Timer2 - TIMSK2 |= (1<direction_bits ^ settings.invert_mask; - st.execute_step = true; // Set flag to set direction bits. - - // Initialize Bresenham variables - st.counter_x = (current_block->step_event_count >> 1); - st.counter_y = st.counter_x; - st.counter_z = st.counter_x; - st.event_count = current_block->step_event_count; - st.step_events_remaining = st.event_count; - - // During feed hold, do not update Ranade counter, rate, or ramp type. Keep decelerating. - if (sys.state == STATE_CYCLE) { - // Initialize Ranade variables - st.d_counter = current_block->d_next; - st.delta_d = current_block->initial_rate; - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; - - // Initialize ramp type. - if (st.step_events_remaining == current_block->decelerate_after) { st.ramp_type = DECEL_RAMP; } - else if (st.delta_d == current_block->nominal_rate) { st.ramp_type = CRUISE_RAMP; } - else { st.ramp_type = ACCEL_RAMP; } - } - - } else { - st_go_idle(); - bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end - return; // Nothing to do but exit. - } - } - - // Adjust inverse time counter for ac/de-celerations - if (st.ramp_type) { - // Tick acceleration ramp counter - st.ramp_count--; - if (st.ramp_count == 0) { - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter - if (st.ramp_type == ACCEL_RAMP) { // Adjust velocity for acceleration - st.delta_d += current_block->rate_delta; - if (st.delta_d >= current_block->nominal_rate) { // Reached cruise state. - st.ramp_type = CRUISE_RAMP; - st.delta_d = current_block->nominal_rate; // Set cruise velocity - } - } else if (st.ramp_type == DECEL_RAMP) { // Adjust velocity for deceleration - if (st.delta_d > current_block->rate_delta) { - st.delta_d -= current_block->rate_delta; - } else { - st.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow. - } - } - } - } - - // Iterate Pramod Ranade inverse time counter. Triggers each Bresenham step event. - if (st.delta_d < MINIMUM_STEP_RATE) { st.d_counter -= MINIMUM_STEP_RATE; } - else { st.d_counter -= st.delta_d; } - - // Execute Bresenham step event, when it's time to do so. - if (st.d_counter < 0) { - st.d_counter += current_block->d_next; - - // Check for feed hold state and execute accordingly. - if (sys.state == STATE_HOLD) { - if (st.ramp_type != DECEL_RAMP) { - st.ramp_type = DECEL_RAMP; - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; - } - if (st.delta_d <= current_block->rate_delta) { - st_go_idle(); - bit_true(sys.execute,EXEC_CYCLE_STOP); - return; - } - } - - // TODO: Vary Bresenham resolution for smoother motions or enable faster step rates (>20kHz). - - out_bits = current_block->direction_bits; // Reset out_bits and reload direction bits - st.execute_step = true; - - // Execute step displacement profile by Bresenham line algorithm - st.counter_x -= current_block->steps[X_AXIS]; - if (st.counter_x < 0) { - out_bits |= (1<steps[Y_AXIS]; - if (st.counter_y < 0) { - out_bits |= (1<steps[Z_AXIS]; - if (st.counter_z < 0) { - out_bits |= (1<decelerate_after) { - st.ramp_type = DECEL_RAMP; - if (st.step_events_remaining == current_block->decelerate_after) { - if (st.delta_d == current_block->nominal_rate) { - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid - } else { - st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; // Set ramp counter for triangle - } - } - } - } - } else { - // If current block is finished, reset pointer - current_block = NULL; - plan_discard_current_block(); - } - - out_bits ^= settings.invert_mask; // Apply step port invert mask - } - busy = false; -// SPINDLE_ENABLE_PORT ^= 1<acceleration = min(block->acceleration,settings.acceleration[idx]*inverse_unit_vec_value); // Incrementally compute cosine of angle between previous and current path. Cos(theta) of the junction @@ -417,7 +417,7 @@ void plan_sync_position() void plan_cycle_reinitialize() { // Re-plan from a complete stop. Reset planner entry speeds and buffer planned pointer. -// st_update_plan_block_parameters(); + st_update_plan_block_parameters(); block_buffer_planned = block_buffer_tail; planner_recalculate(); } diff --git a/planner.h b/planner.h index a1335aa..dc96830 100644 --- a/planner.h +++ b/planner.h @@ -28,25 +28,24 @@ #define BLOCK_BUFFER_SIZE 18 #endif -// This struct is used when buffering the setup for each linear movement "nominal" values are as specified in -// the source g-code and may never actually be reached if acceleration management is active. +// This struct stores a linear movement of a g-code block motion with its critical "nominal" values +// are as specified in the source g-code. typedef struct { - // Fields used by the bresenham algorithm for tracing the line - // NOTE: Do not change any of these values once set. The stepper algorithm uses them to execute the block correctly. - uint8_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) - int32_t steps[N_AXIS]; // Step count along each axis - int32_t step_event_count; // The maximum step axis count and number of steps required to complete this block. + // NOTE: Used by stepper algorithm to execute the block correctly. Do not alter these values. + uint8_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) + int32_t steps[N_AXIS]; // Step count along each axis + int32_t step_event_count; // The maximum step axis count and number of steps required to complete this block. // Fields used by the motion planner to manage acceleration - float entry_speed_sqr; // The current planned entry speed at block junction in (mm/min)^2 - float max_entry_speed_sqr; // Maximum allowable entry speed based on the minimum of junction limit and - // neighboring nominal speeds with overrides in (mm/min)^2 - float max_junction_speed_sqr; // Junction entry speed limit based on direction vectors in (mm/min)^2 - float nominal_speed_sqr; // Axis-limit adjusted nominal speed for this block in (mm/min)^2 - float acceleration; // Axis-limit adjusted line acceleration in (mm/min^2) - float millimeters; // The remaining distance for this block to be executed in (mm) - // uint8_t max_override; // Maximum override value based on axis speed limits + float entry_speed_sqr; // The current planned entry speed at block junction in (mm/min)^2 + float max_entry_speed_sqr; // Maximum allowable entry speed based on the minimum of junction limit and + // neighboring nominal speeds with overrides in (mm/min)^2 + float max_junction_speed_sqr; // Junction entry speed limit based on direction vectors in (mm/min)^2 + float nominal_speed_sqr; // Axis-limit adjusted nominal speed for this block in (mm/min)^2 + float acceleration; // Axis-limit adjusted line acceleration in (mm/min^2) + float millimeters; // The remaining distance for this block to be executed in (mm) + // uint8_t max_override; // Maximum override value based on axis speed limits } plan_block_t; diff --git a/report.c b/report.c index e71ac54..101736a 100644 --- a/report.c +++ b/report.c @@ -151,10 +151,10 @@ void report_grbl_settings() { printPgmString(PSTR("$0=")); printFloat(settings.steps_per_mm[X_AXIS]); printPgmString(PSTR(" (x, step/mm)\r\n$1=")); printFloat(settings.steps_per_mm[Y_AXIS]); printPgmString(PSTR(" (y, step/mm)\r\n$2=")); printFloat(settings.steps_per_mm[Z_AXIS]); - printPgmString(PSTR(" (z, step/mm)\r\n$3=")); printFloat(settings.max_velocity[X_AXIS]); - printPgmString(PSTR(" (x v_max, mm/min)\r\n$4=")); printFloat(settings.max_velocity[Y_AXIS]); - printPgmString(PSTR(" (y v_max, mm/min)\r\n$5=")); printFloat(settings.max_velocity[Z_AXIS]); - printPgmString(PSTR(" (z v_max, mm/min)\r\n$6=")); printFloat(settings.acceleration[X_AXIS]/(60*60)); // Convert from mm/min^2 for human readability + printPgmString(PSTR(" (z, step/mm)\r\n$3=")); printFloat(settings.max_rate[X_AXIS]); + printPgmString(PSTR(" (x max rate, mm/min)\r\n$4=")); printFloat(settings.max_rate[Y_AXIS]); + printPgmString(PSTR(" (y max rate, mm/min)\r\n$5=")); printFloat(settings.max_rate[Z_AXIS]); + printPgmString(PSTR(" (z max rate, mm/min)\r\n$6=")); printFloat(settings.acceleration[X_AXIS]/(60*60)); // Convert from mm/min^2 for human readability printPgmString(PSTR(" (x accel, mm/sec^2)\r\n$7=")); printFloat(settings.acceleration[Y_AXIS]/(60*60)); // Convert from mm/min^2 for human readability printPgmString(PSTR(" (y accel, mm/sec^2)\r\n$8=")); printFloat(settings.acceleration[Z_AXIS]/(60*60)); // Convert from mm/min^2 for human readability printPgmString(PSTR(" (z accel, mm/sec^2)\r\n$9=")); printFloat(-settings.max_travel[X_AXIS]); // Grbl internally store this as negative. diff --git a/serial.c b/serial.c index d3325c7..c07e64b 100644 --- a/serial.c +++ b/serial.c @@ -30,7 +30,7 @@ uint8_t rx_buffer[RX_BUFFER_SIZE]; uint8_t rx_buffer_head = 0; -uint8_t rx_buffer_tail = 0; +volatile uint8_t rx_buffer_tail = 0; uint8_t tx_buffer[TX_BUFFER_SIZE]; uint8_t tx_buffer_head = 0; @@ -93,8 +93,7 @@ void serial_write(uint8_t data) { // Data Register Empty Interrupt handler ISR(SERIAL_UDRE) { - // Temporary tx_buffer_tail (to optimize for volatile) - uint8_t tail = tx_buffer_tail; + uint8_t tail = tx_buffer_tail; // Temporary tx_buffer_tail (to optimize for volatile) #ifdef ENABLE_XONXOFF if (flow_ctrl == SEND_XOFF) { @@ -122,12 +121,15 @@ ISR(SERIAL_UDRE) uint8_t serial_read() { - if (rx_buffer_head == rx_buffer_tail) { + uint8_t tail = rx_buffer_tail; // Temporary rx_buffer_tail (to optimize for volatile) + if (rx_buffer_head == tail) { return SERIAL_NO_DATA; } else { - uint8_t data = rx_buffer[rx_buffer_tail]; - rx_buffer_tail++; - if (rx_buffer_tail == RX_BUFFER_SIZE) { rx_buffer_tail = 0; } + uint8_t data = rx_buffer[tail]; + + tail++; + if (tail == RX_BUFFER_SIZE) { tail = 0; } + rx_buffer_tail = tail; #ifdef ENABLE_XONXOFF if ((get_rx_buffer_count() < RX_BUFFER_LOW) && flow_ctrl == XOFF_SENT) { diff --git a/settings.c b/settings.c index 8f628a5..0ffcc76 100644 --- a/settings.c +++ b/settings.c @@ -73,12 +73,12 @@ void settings_reset(bool reset_all) { settings.steps_per_mm[Z_AXIS] = DEFAULT_Z_STEPS_PER_MM; settings.pulse_microseconds = DEFAULT_STEP_PULSE_MICROSECONDS; settings.default_feed_rate = DEFAULT_FEEDRATE; - settings.max_velocity[X_AXIS] = DEFAULT_RAPID_FEEDRATE; - settings.max_velocity[Y_AXIS] = DEFAULT_RAPID_FEEDRATE; - settings.max_velocity[Z_AXIS] = DEFAULT_RAPID_FEEDRATE; - settings.acceleration[X_AXIS] = DEFAULT_ACCELERATION; - settings.acceleration[Y_AXIS] = DEFAULT_ACCELERATION; - settings.acceleration[Z_AXIS] = DEFAULT_ACCELERATION; + settings.max_rate[X_AXIS] = DEFAULT_X_MAX_RATE; + settings.max_rate[Y_AXIS] = DEFAULT_Y_MAX_RATE; + settings.max_rate[Z_AXIS] = DEFAULT_Z_MAX_RATE; + settings.acceleration[X_AXIS] = DEFAULT_X_ACCELERATION; + settings.acceleration[Y_AXIS] = DEFAULT_Y_ACCELERATION; + settings.acceleration[Z_AXIS] = DEFAULT_Z_ACCELERATION; settings.arc_tolerance = DEFAULT_ARC_TOLERANCE; settings.invert_mask = DEFAULT_STEPPING_INVERT_MASK; settings.junction_deviation = DEFAULT_JUNCTION_DEVIATION; @@ -92,8 +92,8 @@ void settings_reset(bool reset_all) { if (DEFAULT_HARD_LIMIT_ENABLE) { settings.flags |= BITFLAG_HARD_LIMIT_ENABLE; } if (DEFAULT_HOMING_ENABLE) { settings.flags |= BITFLAG_HOMING_ENABLE; } settings.homing_dir_mask = DEFAULT_HOMING_DIR_MASK; - settings.homing_feed_rate = DEFAULT_HOMING_FEEDRATE; - settings.homing_seek_rate = DEFAULT_HOMING_RAPID_FEEDRATE; + settings.homing_feed_rate = DEFAULT_HOMING_FEED_RATE; + settings.homing_seek_rate = DEFAULT_HOMING_SEEK_RATE; settings.homing_debounce_delay = DEFAULT_HOMING_DEBOUNCE_DELAY; settings.homing_pulloff = DEFAULT_HOMING_PULLOFF; settings.stepper_idle_lock_time = DEFAULT_STEPPER_IDLE_LOCK_TIME; @@ -163,9 +163,9 @@ uint8_t settings_store_global_setting(int parameter, float value) { case 0: case 1: case 2: if (value <= 0.0) { return(STATUS_SETTING_VALUE_NEG); } settings.steps_per_mm[parameter] = value; break; - case 3: settings.max_velocity[X_AXIS] = value; break; - case 4: settings.max_velocity[Y_AXIS] = value; break; - case 5: settings.max_velocity[Z_AXIS] = value; break; + case 3: settings.max_rate[X_AXIS] = value; break; + case 4: settings.max_rate[Y_AXIS] = value; break; + case 5: settings.max_rate[Z_AXIS] = value; break; case 6: settings.acceleration[X_AXIS] = value*60*60; break; // Convert to mm/min^2 for grbl internal use. case 7: settings.acceleration[Y_AXIS] = value*60*60; break; // Convert to mm/min^2 for grbl internal use. case 8: settings.acceleration[Z_AXIS] = value*60*60; break; // Convert to mm/min^2 for grbl internal use. diff --git a/settings.h b/settings.h index 3369845..6b7d36f 100644 --- a/settings.h +++ b/settings.h @@ -25,11 +25,11 @@ #include #include "nuts_bolts.h" -#define GRBL_VERSION "0.9a" +#define GRBL_VERSION "0.9b" // Version of the EEPROM data. Will be used to migrate existing data from older versions of Grbl // when firmware is upgraded. Always stored in byte 0 of eeprom -#define SETTINGS_VERSION 53 +#define SETTINGS_VERSION 55 // Define bit flag masks for the boolean settings in settings.flag. #define BITFLAG_REPORT_INCHES bit(0) @@ -58,24 +58,22 @@ // Global persistent settings (Stored from byte EEPROM_ADDR_GLOBAL onwards) typedef struct { float steps_per_mm[N_AXIS]; - uint8_t microsteps; + float max_rate[N_AXIS]; + float acceleration[N_AXIS]; + float max_travel[N_AXIS]; uint8_t pulse_microseconds; float default_feed_rate; - float default_seek_rate; uint8_t invert_mask; - float arc_tolerance; - float acceleration[N_AXIS]; + uint8_t stepper_idle_lock_time; // If max value 255, steppers do not disable. float junction_deviation; + float arc_tolerance; + uint8_t decimal_places; uint8_t flags; // Contains default boolean settings uint8_t homing_dir_mask; float homing_feed_rate; float homing_seek_rate; uint16_t homing_debounce_delay; float homing_pulloff; - uint8_t stepper_idle_lock_time; // If max value 255, steppers do not disable. - uint8_t decimal_places; - float max_velocity[N_AXIS]; - float max_travel[N_AXIS]; // uint8_t status_report_mask; // Mask to indicate desired report data. } settings_t; extern settings_t settings; diff --git a/stepper.c b/stepper.c index 771480d..c2813a9 100644 --- a/stepper.c +++ b/stepper.c @@ -105,10 +105,9 @@ typedef struct { float step_per_mm; // Current planner block step/millimeter conversion scalar float steps_remaining; - int32_t step_events_remaining; // Tracks step event count for the executing planner block uint8_t ramp_type; // Current segment ramp state - float millimeters_remaining; + float mm_eob; float current_speed; // Current speed at the end of the segment buffer (mm/min) float maximum_speed; // Maximum speed of executing block. Not always nominal speed. (mm/min) float exit_speed; // Exit speed of executing block (mm/min) @@ -217,15 +216,14 @@ void st_go_idle() stepper pins appropriately. It is supported by The Stepper Port Reset Interrupt which it uses to reset the stepper port after each pulse. The bresenham line tracer algorithm controls all three stepper outputs simultaneously with these two interrupts. + + NOTE: This interrupt must be as efficient as possible and complete before the next ISR tick, + which for Grbl is 33.3usec at a 30kHz ISR rate. Oscilloscope measured time in ISR is 5usec + typical and 25usec maximum, well below requirement. */ -/* TODO: - - Measured time in ISR. Typical and worst-case. Roughly 5usec min to 25usec max. Good enough. - There are no major changes to the base operations of this ISR with the new segment buffer. - - Determine if placing the position counters elsewhere (or change them to 8-bit variables that - are added to the system position counters at the end of a segment) frees up cycles. - - Create NOTE: to describe that the total time in this ISR must be less than the ISR frequency - in its worst case scenario. -*/ +// TODO: Replace direct updating of the int32 position counters in the ISR somehow. Perhaps use smaller +// int8 variables and update position counters only when a segment completes. This can get complicated +// with probing and homing cycles that require true real-time positions. ISR(TIMER2_COMPA_vect) { // SPINDLE_ENABLE_PORT ^= 1<millimeters = prep.millimeters_remaining; pl_block->entry_speed_sqr = prep.current_speed*prep.current_speed; // Update entry speed. pl_block = NULL; // Flag st_prep_segment() to load new velocity profile. } @@ -456,32 +453,12 @@ void st_update_plan_block_parameters() accounted for. This allows the stepper algorithm to run at very high step rates without losing steps. */ -/* - TODO: With feedrate overrides, increases to the override value will not significantly - change the planner and stepper current operation. When the value increases, we simply - need to recompute the block plan with new nominal speeds and maximum junction velocities. - However with a decreasing feedrate override, this gets a little tricky. The current block - plan is optimal, so if we try to reduce the feed rates, it may be impossible to create - a feasible plan at its current operating speed and decelerate down to zero at the end of - the buffer. We first have to enforce a deceleration to meet and intersect with the reduced - feedrate override plan. For example, if the current block is cruising at a nominal rate - and the feedrate override is reduced, the new nominal rate will now be lower. The velocity - profile must first decelerate to the new nominal rate and then follow on the new plan. - Another issue is whether or not a feedrate override reduction causes a deceleration - that acts over several planner blocks. For example, say that the plan is already heavily - decelerating throughout it, reducing the feedrate override will not do much to it. So, - how do we determine when to resume the new plan? One solution is to tie into the feed hold - handling code to enforce a deceleration, but check when the current speed is less than or - equal to the block maximum speed and is in an acceleration or cruising ramp. At this - point, we know that we can recompute the block velocity profile to meet and continue onto - the new block plan. -*/ void st_prep_buffer() { while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer. if (sys.state == STATE_QUEUED) { return; } // Block until a motion state is issued - // Determine if we need to load a new planner block. If so, prepare step data. + // Determine if we need to load a new planner block or if the block remainder is replanned. if (pl_block == NULL) { pl_block = plan_get_current_block(); // Query planner for a queued block if (pl_block == NULL) { return; } // No planner blocks. Exit. @@ -504,10 +481,8 @@ void st_prep_buffer() st_prep_block->step_event_count = pl_block->step_event_count; // Initialize segment buffer data for generating the segments. - prep.step_events_remaining = st_prep_block->step_event_count; prep.steps_remaining = st_prep_block->step_event_count; - prep.millimeters_remaining = pl_block->millimeters; - prep.step_per_mm = prep.steps_remaining/prep.millimeters_remaining; + prep.step_per_mm = prep.steps_remaining/pl_block->millimeters; if (sys.state == STATE_HOLD) { prep.current_speed = prep.exit_speed; @@ -516,28 +491,30 @@ void st_prep_buffer() else { prep.current_speed = sqrt(pl_block->entry_speed_sqr); } } + prep.mm_eob = 0.0; + float inv_2_accel = 0.5/pl_block->acceleration; if (sys.state == STATE_HOLD) { - // Compute velocity profile parameters for a feed hold in-progress. + // Compute velocity profile parameters for a feed hold in-progress. This profile overrides + // the planner block profile, enforcing a deceleration to zero speed. prep.ramp_type = RAMP_DECEL; float decel_dist = inv_2_accel*pl_block->entry_speed_sqr; - if (decel_dist < prep.millimeters_remaining) { + if (decel_dist < pl_block->millimeters) { prep.exit_speed = 0.0; - prep.steps_remaining = prep.step_per_mm*decel_dist; - prep.millimeters_remaining = decel_dist; + prep.mm_eob = pl_block->millimeters-decel_dist; } else { - prep.exit_speed = sqrt(pl_block->entry_speed_sqr-2*pl_block->acceleration*prep.millimeters_remaining); + prep.exit_speed = sqrt(pl_block->entry_speed_sqr-2*pl_block->acceleration*pl_block->millimeters); } } else { // Compute velocity profile parameters of the prepped planner block. prep.ramp_type = RAMP_ACCEL; // Initialize as acceleration ramp. - prep.accelerate_until = prep.millimeters_remaining; + prep.accelerate_until = pl_block->millimeters; prep.exit_speed = plan_get_exec_block_exit_speed(); float exit_speed_sqr = prep.exit_speed*prep.exit_speed; float intersect_distance = - 0.5*(prep.millimeters_remaining+inv_2_accel*(pl_block->entry_speed_sqr-exit_speed_sqr)); + 0.5*(pl_block->millimeters+inv_2_accel*(pl_block->entry_speed_sqr-exit_speed_sqr)); if (intersect_distance > 0.0) { - if (intersect_distance < prep.millimeters_remaining) { // Either trapezoid or triangle types + if (intersect_distance < pl_block->millimeters) { // Either trapezoid or triangle types // NOTE: For acceleration-cruise and cruise-only types, following calculation will be 0.0. prep.decelerate_after = inv_2_accel*(pl_block->nominal_speed_sqr-exit_speed_sqr); if (prep.decelerate_after < intersect_distance) { // Trapezoid type @@ -556,16 +533,15 @@ void st_prep_buffer() } } else { // Deceleration-only type prep.ramp_type = RAMP_DECEL; - prep.decelerate_after = prep.millimeters_remaining; + // prep.decelerate_after = pl_block->millimeters; prep.maximum_speed = prep.current_speed; } } else { // Acceleration-only type prep.accelerate_until = 0.0; - prep.decelerate_after = 0.0; + // prep.decelerate_after = 0.0; prep.maximum_speed = prep.exit_speed; } - } - + } } // Initialize new segment @@ -584,18 +560,18 @@ void st_prep_buffer() considered completed despite having a truncated execution time less than DT_SEGMENT. */ float dt = 0.0; - float mm_remaining = prep.millimeters_remaining; + float mm_remaining = pl_block->millimeters; float time_var = DT_SEGMENT; // Time worker variable float mm_var; // mm distance worker variable do { switch (prep.ramp_type) { case RAMP_ACCEL: - // NOTE: Acceleration ramp always computes during first loop only. + // NOTE: Acceleration ramp only computes during first do-while loop. mm_remaining -= DT_SEGMENT*(prep.current_speed + pl_block->acceleration*(0.5*DT_SEGMENT)); if (mm_remaining < prep.accelerate_until) { // End of acceleration ramp. // Acceleration-cruise, acceleration-deceleration ramp junction, or end of block. mm_remaining = prep.accelerate_until; // NOTE: 0.0 at EOB - time_var = 2.0*(prep.millimeters_remaining-mm_remaining)/(prep.current_speed+prep.maximum_speed); + time_var = 2.0*(pl_block->millimeters-mm_remaining)/(prep.current_speed+prep.maximum_speed); if (mm_remaining == prep.decelerate_after) { prep.ramp_type = RAMP_DECEL; } else { prep.ramp_type = RAMP_CRUISE; } prep.current_speed = prep.maximum_speed; @@ -618,21 +594,21 @@ void st_prep_buffer() default: // case RAMP_DECEL: // NOTE: mm_var used to catch negative decelerate distance values near zero speed. mm_var = time_var*(prep.current_speed - 0.5*pl_block->acceleration*time_var); - if ((mm_var > 0.0) && (mm_var < mm_remaining)) { // Deceleration only. + if ((mm_var > prep.mm_eob) && (mm_var < mm_remaining)) { // Deceleration only. prep.current_speed -= pl_block->acceleration*time_var; // Check for near-zero speed and prevent divide by zero in rare scenarios. if (prep.current_speed > prep.exit_speed) { mm_remaining -= mm_var; } - else { mm_remaining = 0.0; } // NOTE: Force EOB for now. May or may not be needed. + else { mm_remaining = prep.mm_eob; } // NOTE: Force EOB for now. May or may not be needed. } else { // End of block. - time_var = 2.0*mm_remaining/(prep.current_speed+prep.exit_speed); - mm_remaining = 0.0; + time_var = 2.0*(mm_remaining-prep.mm_eob)/(prep.current_speed+prep.exit_speed); + mm_remaining = prep.mm_eob; // prep.current_speed = prep.exit_speed; // !! May be needed for feed hold reinitialization. } } dt += time_var; // Add computed ramp time to total segment time. if (dt < DT_SEGMENT) { time_var = DT_SEGMENT - dt; } // **Incomplete** At ramp junction. else { break; } // **Complete** Exit loop. Segment execution time maxed. - } while ( mm_remaining > 0.0 ); // **Complete** Exit loop. End of planner block. + } while (mm_remaining > prep.mm_eob); // **Complete** Exit loop. End of planner block. /* ----------------------------------------------------------------------------------- Compute segment step rate, steps to execute, and step phase correction parameters. @@ -654,38 +630,37 @@ void st_prep_buffer() prep_segment->phase_dist = ceil(INV_TIME_MULTIPLIER*(ceil(steps_remaining)-steps_remaining)); prep_segment->n_step = ceil(prep.steps_remaining)-ceil(steps_remaining); - // Update step execution variables - prep.step_events_remaining -= prep_segment->n_step; - prep.millimeters_remaining = mm_remaining; - prep.steps_remaining = steps_remaining; + // Update step execution variables. + if (mm_remaining == prep.mm_eob) { + // NOTE: Currently only feed holds qualify for this scenario. May change with overrides. + prep.current_speed = 0.0; + prep.steps_remaining = ceil(steps_remaining); + pl_block->millimeters = prep.steps_remaining/prep.step_per_mm; + plan_cycle_reinitialize(); + sys.state = STATE_QUEUED; // End cycle. + } else { + pl_block->millimeters = mm_remaining; + prep.steps_remaining = steps_remaining; + } } else { // End of block. // Set to execute the remaining steps and no phase correction upon finishing the block. prep_segment->dist_per_tick = ceil( prep.steps_remaining*time_var ); // (mult*step/isr_tic) prep_segment->phase_dist = 0; prep_segment->n_step = ceil(prep.steps_remaining); + + // The planner block is complete. All steps are set to be executed in the segment buffer. + // TODO: Broken with feed holds. Need to recalculate the planner buffer at this time. + pl_block = NULL; + plan_discard_current_block(); - prep.step_events_remaining -= prep_segment->n_step; - if (prep.step_events_remaining > 0) { - sys.state = STATE_QUEUED; - pl_block->entry_speed_sqr = 0.0; - prep.current_speed = 0.0; - prep.steps_remaining = prep.step_events_remaining; - pl_block->millimeters = prep.steps_remaining/prep.step_per_mm; - prep.millimeters_remaining = pl_block->millimeters; - pl_block = NULL; - prep.flag_partial_block = true; - plan_cycle_reinitialize(); - } else { - // The planner block is complete. All steps are set to be executed in the segment buffer. - // TODO: Ignore this for feed holds. Need to recalculate the planner buffer at this time. - pl_block = NULL; - plan_discard_current_block(); + if (sys.state == STATE_HOLD) { + if (prep.current_speed == 0.0) { + plan_cycle_reinitialize(); + sys.state = STATE_QUEUED; + } } - } - - // New step segment initialization completed. Increment segment buffer indices. segment_buffer_head = segment_next_head; @@ -696,3 +671,24 @@ void st_prep_buffer() // printInteger(blength); } } + +/* + TODO: With feedrate overrides, increases to the override value will not significantly + change the planner and stepper current operation. When the value increases, we simply + need to recompute the block plan with new nominal speeds and maximum junction velocities. + However with a decreasing feedrate override, this gets a little tricky. The current block + plan is optimal, so if we try to reduce the feed rates, it may be impossible to create + a feasible plan at its current operating speed and decelerate down to zero at the end of + the buffer. We first have to enforce a deceleration to meet and intersect with the reduced + feedrate override plan. For example, if the current block is cruising at a nominal rate + and the feedrate override is reduced, the new nominal rate will now be lower. The velocity + profile must first decelerate to the new nominal rate and then follow on the new plan. + Another issue is whether or not a feedrate override reduction causes a deceleration + that acts over several planner blocks. For example, say that the plan is already heavily + decelerating throughout it, reducing the feedrate override will not do much to it. So, + how do we determine when to resume the new plan? One solution is to tie into the feed hold + handling code to enforce a deceleration, but check when the current speed is less than or + equal to the block maximum speed and is in an acceleration or cruising ramp. At this + point, we know that we can recompute the block velocity profile to meet and continue onto + the new block plan. +*/ diff --git a/stepper_test.c b/stepper_test.c deleted file mode 100644 index fea6315..0000000 --- a/stepper_test.c +++ /dev/null @@ -1,788 +0,0 @@ -/* - stepper.c - stepper motor driver: executes motion plans using stepper motors - Part of Grbl - - Copyright (c) 2011-2013 Sungeun K. Jeon - Copyright (c) 2009-2011 Simen Svale Skogsrud - - 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 - (at your option) any later version. - - Grbl is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with Grbl. If not, see . -*/ - -#include -#include "stepper.h" -#include "config.h" -#include "settings.h" -#include "planner.h" -#include "nuts_bolts.h" - -// Some useful constants -#define TICKS_PER_MICROSECOND (F_CPU/1000000) - -#define RAMP_NOOP_CRUISE 0 -#define RAMP_ACCEL 1 -#define RAMP_DECEL 2 - -#define LOAD_NOOP 0 -#define LOAD_SEGMENT 1 -#define LOAD_BLOCK 2 - -#define SEGMENT_NOOP 0 -#define SEGMENT_END_OF_BLOCK bit(0) -#define RAMP_CHANGE_ACCEL bit(1) -#define RAMP_CHANGE_DECEL bit(2) - -#define MINIMUM_STEPS_PER_SEGMENT 1 // Don't change - -#define SEGMENT_BUFFER_SIZE 6 - -#define DT_SEGMENT (1/ACCELERATION_TICKS_PER_SECOND) - -// Stepper state variable. Contains running data and trapezoid variables. -typedef struct { - // Used by the bresenham line algorithm - int32_t counter_x, // Counter variables for the bresenham line tracer - counter_y, - counter_z; - - // Used by inverse time algorithm to track step rate - int32_t counter_dist; // Inverse time distance traveled since last step event - - uint8_t step_count; // Steps remaining in line segment motion - uint8_t phase_count; // Phase ticks remaining after line segment steps complete - - // Used by the stepper driver interrupt - uint8_t execute_step; // Flags step execution for each interrupt. - uint8_t step_pulse_time; // Step pulse reset time after step rise - uint8_t out_bits; // The next stepping-bits to be output - uint8_t load_flag; -} stepper_t; -static stepper_t st; - -// Stores stepper common data for executing steps in the segment buffer. Data can change mid-block when the -// planner updates the remaining block velocity profile with a more optimal plan or a feedrate override occurs. -// NOTE: Normally, this buffer is partially in-use, but, for the worst case scenario, it will never exceed -// the number of accessible stepper buffer segments (SEGMENT_BUFFER_SIZE-1). -typedef struct { - - // TODO: Retain step[N_AXIS], step_event_count, and direction byte here, so that we can throw - // away the planner block when the segment prep is complete. - - float step_events_remaining; // Tracks step event count for the executing planner block - - // Planner block velocity profile parameters used to trace and execute steps.; - float accelerate_until; - float decelerate_after; - float current_speed; - float maximum_speed; - float exit_speed; -} st_data_t; -static st_data_t segment_data[SEGMENT_BUFFER_SIZE-1]; - -// Primary stepper segment ring buffer. Contains small, short line segments for the stepper algorithm to execute, -// which are "checked-out" incrementally from the first block in the planner buffer. Once "checked-out", the steps -// in the segments buffer cannot be modified by the planner, where the remaining planner block steps still can. -typedef struct { - uint8_t n_step; // Number of step events to be executed for this segment - uint8_t n_phase_tick; - uint32_t dist_per_tick; - uint8_t st_data_index; // Stepper buffer common data index. Uses this information to execute this segment. - uint8_t flag; // Stepper algorithm bit-flag for special execution conditions. -} st_segment_t; -static st_segment_t segment_buffer[SEGMENT_BUFFER_SIZE]; - -// Step segment ring buffer indices -static volatile uint8_t segment_buffer_tail; -static volatile uint8_t segment_buffer_head; -static uint8_t segment_next_head; - -static volatile uint8_t busy; // Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though. -static plan_block_t *pl_current_block; // A pointer to the planner block currently being traced -static st_segment_t *st_current_segment; -static st_data_t *st_current_data; - -// Pointers for the step segment being prepped from the planner buffer. Accessed only by the -// main program. Pointers may be planning segments or planner blocks ahead of what being executed. -static plan_block_t *pl_prep_block; // Pointer to the planner block being prepped -static st_data_t *st_prep_data; // Pointer to the stepper common data being prepped -static uint8_t pl_prep_index; // Index of planner block being prepped -static uint8_t st_data_prep_index; // Index of stepper common data block being prepped -static uint8_t pl_partial_block_flag; // Flag indicating the planner has modified the prepped planner block - -static float st_prep_step_per_mm; - -// TODO: All this stuff only needs to be retained for the prepped planner block. Once changed -// or complete, we do not need this information anymore. Duh! -// typedef struct { -// float step_events_remaining; // Tracks step event count for the executing planner block -// float accelerate_until; -// float decelerate_after; -// float current_speed; -// float maximum_speed; -// float exit_speed; -// float step_per_mm; -// } st_prep_data_t; - -/* __________________________ - /| |\ _________________ ^ - / | | \ /| |\ | - / | | \ / | | \ s - / | | | | | \ p - / | | | | | \ e - +-----+------------------------+---+--+---------------+----+ e - | BLOCK 1 | BLOCK 2 | d - - time -----> - - The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta - until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after - after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as - +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND. -*/ - -// Stepper state initialization. Cycle should only start if the st.cycle_start flag is -// enabled. Startup init and limits call this function but shouldn't start the cycle. -void st_wake_up() -{ - // Enable steppers by resetting the stepper disable port - if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { - STEPPERS_DISABLE_PORT |= (1<> 3); - // Enable stepper driver interrupt - st.execute_step = false; - st.load_flag = LOAD_BLOCK; - - TCNT2 = 0; // Clear Timer2 - TIMSK2 |= (1<n_step; - - // If the new segment starts a new planner block, initialize stepper variables and counters. - // NOTE: For new segments only, the step counters are not updated to ensure step phasing is continuous. - if (st.load_flag == LOAD_BLOCK) { - pl_current_block = plan_get_current_block(); // Should always be there. Stepper buffer handles this. - st_current_data = &segment_data[segment_buffer[segment_buffer_tail].st_data_index]; - - // Initialize direction bits for block. Set execute flag to set directions bits upon next ISR tick. - st.out_bits = pl_current_block->direction_bits ^ settings.invert_mask; - st.execute_step = true; - - // Initialize Bresenham line counters - st.counter_x = (pl_current_block->step_event_count >> 1); - st.counter_y = st.counter_x; - st.counter_z = st.counter_x; - - // Initialize inverse time, step rate data, and acceleration ramp counters - st.counter_dist = INV_TIME_MULTIPLIER; // dist_per_step always greater than dist_per_tick. - } - - st.load_flag = LOAD_NOOP; // Segment motion loaded. Set no-operation flag to skip during execution. - - } else { - // Can't discard planner block here if a feed hold stops in middle of block. - st_go_idle(); - bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end - return; // Nothing to do but exit. - } - - } - - // Iterate inverse time counter. Triggers each Bresenham step event. - st.counter_dist -= st_current_segment->dist_per_tick; - - // Execute Bresenham step event, when it's time to do so. - if (st.counter_dist < 0) { - if (st.step_count > 0) { // Block phase correction from executing step. - st.counter_dist += INV_TIME_MULTIPLIER; // Reload inverse time counter - - st.out_bits = pl_current_block->direction_bits; // Reset out_bits and reload direction bits - st.execute_step = true; - - // Execute step displacement profile by Bresenham line algorithm - st.counter_x -= pl_current_block->steps[X_AXIS]; - if (st.counter_x < 0) { - st.out_bits |= (1<step_event_count; - if (st.out_bits & (1<steps[Y_AXIS]; - if (st.counter_y < 0) { - st.out_bits |= (1<step_event_count; - if (st.out_bits & (1<steps[Z_AXIS]; - if (st.counter_z < 0) { - st.out_bits |= (1<step_event_count; - if (st.out_bits & (1<flag & SEGMENT_END_OF_BLOCK) { - plan_discard_current_block(); - st.load_flag = LOAD_BLOCK; - } else { - st.load_flag = LOAD_SEGMENT; - } - - // Discard current segment by advancing buffer tail index - if ( ++segment_buffer_tail == SEGMENT_BUFFER_SIZE) { segment_buffer_tail = 0; } - } - st.phase_count--; - } - - - busy = false; -// SPINDLE_ENABLE_PORT ^= 1<step_events_remaining); -// st.ramp_type = RAMP_ACCEL; -// st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK/2; -// st.ramp_rate = 0; -// sys.state = STATE_QUEUED; -// } else { -// sys.state = STATE_IDLE; -// } - sys.state = STATE_IDLE; - -} - - -/* Prepares step segment buffer. Continuously called from main program. - - The segment buffer is an intermediary buffer interface between the execution of steps - by the stepper algorithm and the velocity profiles generated by the planner. The stepper - algorithm only executes steps within the segment buffer and is filled by the main program - when steps are "checked-out" from the first block in the planner buffer. This keeps the - step execution and planning optimization processes atomic and protected from each other. - The number of steps "checked-out" from the planner buffer and the number of segments in - the segment buffer is sized and computed such that no operation in the main program takes - longer than the time it takes the stepper algorithm to empty it before refilling it. - Currently, the segment buffer conservatively holds roughly up to 40-60 msec of steps. - - NOTE: The segment buffer executes a set number of steps over an approximate time period. - If we try to execute over a fixed time period, it is difficult to guarantee or predict - how many steps will execute over it, especially when the step pulse phasing between the - neighboring segments must also be kept consistent. Meaning that, if the last segment step - pulses right before a segment end, the next segment must delay its first pulse so that the - step pulses are consistently spaced apart over time to keep the step pulse train nice and - smooth. Keeping track of phasing and ensuring that the exact number of steps are executed - as defined by the planner block, the related computational overhead can get quickly and - prohibitively expensive, especially in real-time. - Since the stepper algorithm automatically takes care of the step pulse phasing with - its ramp and inverse time counters by retaining the count remainders, we don't have to - explicitly and expensively track and synchronize the exact number of steps, time, and - phasing of steps. All we need to do is approximate the number of steps in each segment - such that the segment buffer has enough execution time for the main program to do what - it needs to do and refill it when it comes back. In other words, we just need to compute - a cheap approximation of the current velocity and the number of steps over it. -*/ - -/* - TODO: Figure out how to enforce a deceleration when a feedrate override is reduced. - The problem is that when an override is reduced, the planner may not plan back to - the current rate. Meaning that the velocity profiles for certain conditions no longer - are trapezoidal or triangular. For example, if the current block is cruising at a - nominal rate and the feedrate override is reduced, the new nominal rate will now be - lower. The velocity profile must first decelerate to the new nominal rate and then - follow on the new plan. So the remaining velocity profile will have a decelerate, - cruise, and another decelerate. - Another issue is whether or not a feedrate override reduction causes a deceleration - that acts over several planner blocks. For example, say that the plan is already - heavily decelerating throughout it, reducing the feedrate will not do much to it. So, - how do we determine when to resume the new plan? How many blocks do we have to wait - until the new plan intersects with the deceleration curve? One plus though, the - deceleration will never be more than the number of blocks in the entire planner buffer, - but it theoretically can be equal to it when all planner blocks are decelerating already. -*/ -void st_prep_buffer() -{ - if (sys.state == STATE_QUEUED) { return; } // Block until a motion state is issued - while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer. - - // Initialize new segment - st_segment_t *prep_segment = &segment_buffer[segment_buffer_head]; - prep_segment->flag = SEGMENT_NOOP; - - // ----------------------------------------------------------------------------------- - // Determine if we need to load a new planner block. If so, prepare step data. - if (pl_prep_block == NULL) { - pl_prep_block = plan_get_block_by_index(pl_prep_index); // Query planner for a queued block - if (pl_prep_block == NULL) { return; } // No planner blocks. Exit. - - // Increment stepper common data index - if ( ++st_data_prep_index == (SEGMENT_BUFFER_SIZE-1) ) { st_data_prep_index = 0; } - - // Check if the planner has re-computed this block mid-execution. If so, push the previous - // segment data. Otherwise, prepare a new segment data for the new planner block. - if (pl_partial_block_flag) { - - // Prepare new shared segment block data and copy the relevant last segment block data. - st_data_t *last_st_prep_data; - last_st_prep_data = st_prep_data; - st_prep_data = &segment_data[st_data_prep_index]; - - st_prep_data->step_events_remaining = last_st_prep_data->step_events_remaining; - - pl_partial_block_flag = false; // Reset flag - - } else { - - // Prepare commonly shared planner block data for the ensuing segment buffer moves ad-hoc, since - // the planner buffer can dynamically change the velocity profile data as blocks are added. - st_prep_data = &segment_data[st_data_prep_index]; - - st_prep_step_per_mm = pl_prep_block->step_event_count/pl_prep_block->millimeters; - - // Initialize planner block step data - st_prep_data->step_events_remaining = pl_prep_block->step_event_count; - - } - - - st_prep_data->current_speed = sqrt(pl_prep_block->entry_speed_sqr); - - // Determine current block exit speed - plan_block_t *pl_next_block = plan_get_block_by_index(plan_next_block_index(pl_prep_index)); - float exit_speed_sqr; - if (pl_next_block == NULL) { - exit_speed_sqr = 0.0; - st_prep_data->exit_speed = 0.0; - } else { - exit_speed_sqr = pl_next_block->entry_speed_sqr; - st_prep_data->exit_speed = sqrt(exit_speed_sqr); - } - -// st_prep_data->accelerate_until = 0.5*(pl_prep_block->nominal_speed_sqr-pl_prep_block->entry_speed_sqr)/(pl_prep_block->acceleration); -// st_prep_data->decelerate_after = 0.5*(pl_prep_block->nominal_speed_sqr-exit_speed_sqr)/(pl_prep_block->acceleration); -// if (pl_prep_block->millimeters < st_prep_data->accelerate_until+st_prep_data->decelerate_after) { -// st_prep_data->decelerate_after = 0.5*( pl_prep_block->millimeters + 0.5*(pl_prep_block->entry_speed_sqr -// - exit_speed_sqr)/(pl_prep_block->acceleration) ); -// st_prep_data->accelerate_until = pl_prep_block->millimeters-st_prep_data->decelerate_after; -// st_prep_data->maximum_speed = sqrt(2*pl_prep_block->acceleration*st_prep_data->decelerate_after+exit_speed_sqr); -// } else { -// st_prep_data->accelerate_until = pl_prep_block->millimeters-st_prep_data->accelerate_until; -// st_prep_data->maximum_speed = sqrt(pl_prep_block->nominal_speed_sqr); -// } - - // Determine velocity profile based on the 7 possible types: Cruise-only, cruise-deceleration, - // acceleration-cruise, acceleration-only, deceleration-only, trapezoid, and triangle. - st_prep_data->accelerate_until = pl_prep_block->millimeters; - if (pl_prep_block->entry_speed_sqr == pl_prep_block->nominal_speed_sqr) { - st_prep_data->maximum_speed = sqrt(pl_prep_block->nominal_speed_sqr); - if (exit_speed_sqr == pl_prep_block->nominal_speed_sqr) { // Cruise-only type - st_prep_data->decelerate_after = 0.0; - } else { // Cruise-deceleration type - st_prep_data->decelerate_after = 0.5*(pl_prep_block->nominal_speed_sqr-exit_speed_sqr)/(pl_prep_block->acceleration); - } - } else if (exit_speed_sqr == pl_prep_block->nominal_speed_sqr) { - // Acceleration-cruise type - st_prep_data->maximum_speed = sqrt(pl_prep_block->nominal_speed_sqr); - st_prep_data->decelerate_after = 0.0; - st_prep_data->accelerate_until -= 0.5*(pl_prep_block->nominal_speed_sqr-pl_prep_block->entry_speed_sqr)/(pl_prep_block->acceleration); - } else { - float intersection_dist = 0.5*( pl_prep_block->millimeters + 0.5*(pl_prep_block->entry_speed_sqr - - exit_speed_sqr)/(pl_prep_block->acceleration) ); - if (intersection_dist > 0.0) { - if (intersection_dist < pl_prep_block->millimeters) { // Either trapezoid or triangle types - st_prep_data->decelerate_after = 0.5*(pl_prep_block->nominal_speed_sqr-exit_speed_sqr)/(pl_prep_block->acceleration); - if (st_prep_data->decelerate_after < intersection_dist) { // Trapezoid type - st_prep_data->maximum_speed = sqrt(pl_prep_block->nominal_speed_sqr); - st_prep_data->accelerate_until -= 0.5*(pl_prep_block->nominal_speed_sqr-pl_prep_block->entry_speed_sqr)/(pl_prep_block->acceleration); - } else { // Triangle type - st_prep_data->decelerate_after = intersection_dist; - st_prep_data->maximum_speed = sqrt(2*pl_prep_block->acceleration*st_prep_data->decelerate_after+exit_speed_sqr); - st_prep_data->accelerate_until -= st_prep_data->decelerate_after; - } - } else { // Deceleration-only type - st_prep_data->maximum_speed = st_prep_data->current_speed; - st_prep_data->decelerate_after = pl_prep_block->millimeters; - } - } else { // Acceleration-only type - st_prep_data->maximum_speed = st_prep_data->exit_speed; - st_prep_data->decelerate_after = 0.0; - st_prep_data->accelerate_until = 0.0; - } - } - } - - // Set new segment to point to the current segment data block. - prep_segment->st_data_index = st_data_prep_index; - - // ----------------------------------------------------------------------------------- - // Initialize segment execute distance. Attempt to create a full segment over DT_SEGMENT. - - float mm_remaining = pl_prep_block->millimeters; - float dt = DT_SEGMENT; - if (mm_remaining > st_prep_data->accelerate_until) { // Acceleration ramp - mm_remaining -= (st_prep_data->current_speed*DT_SEGMENT - + pl_prep_block->acceleration*(0.5*DT_SEGMENT*DT_SEGMENT)); - if (mm_remaining < st_prep_data->accelerate_until) { // **Incomplete** Acceleration ramp end. - // Acceleration-cruise, acceleration-deceleration ramp junction, or end of block. - mm_remaining = st_prep_data->accelerate_until; // NOTE: 0.0 at EOB - dt = 2*(pl_prep_block->millimeters-mm_remaining)/ - (st_prep_data->current_speed+st_prep_data->maximum_speed); - st_prep_data->current_speed = st_prep_data->maximum_speed; - } else { // **Complete** Acceleration only. - st_prep_data->current_speed += pl_prep_block->acceleration*DT_SEGMENT; - } - } else if (mm_remaining <= st_prep_data->decelerate_after) { // Deceleration ramp - mm_remaining -= (st_prep_data->current_speed*DT_SEGMENT - - pl_prep_block->acceleration*(0.5*DT_SEGMENT*DT_SEGMENT)); - if (mm_remaining > 0.0) { // **Complete** Deceleration only. - st_prep_data->current_speed -= pl_prep_block->acceleration*DT_SEGMENT; - } else { // **Complete* End of block. - dt = 2*pl_prep_block->millimeters/(st_prep_data->current_speed+st_prep_data->exit_speed); - mm_remaining = 0.0; - // st_prep_data->current_speed = st_prep_data->exit_speed; - } - } else { // Cruising profile - mm_remaining -= st_prep_data->maximum_speed*DT_SEGMENT; - if (mm_remaining < st_prep_data->decelerate_after) { // **Incomplete** End of cruise. - // Cruise-deceleration junction or end of block. - mm_remaining = st_prep_data->decelerate_after; // NOTE: 0.0 at EOB - dt = (pl_prep_block->millimeters-mm_remaining)/st_prep_data->maximum_speed; - } // Otherwise **Complete** Cruising only. - } - - // ----------------------------------------------------------------------------------- - // If segment is incomplete, attempt to fill the remainder. - // NOTE: Segment remainder always spans a cruise and/or a deceleration ramp. - - if (dt < DT_SEGMENT) { - if (mm_remaining > 0.0) { // Skip if end of block. - float last_mm_remaining; - float dt_remainder; - - // Fill incomplete segment with an acceleration junction. - if (mm_remaining > st_prep_data->decelerate_after) { // Cruising profile - last_mm_remaining = mm_remaining; - dt_remainder = DT_SEGMENT-dt; - mm_remaining -= st_prep_data->current_speed*dt_remainder; - if (mm_remaining < st_prep_data->decelerate_after) { // **Incomplete** - mm_remaining = st_prep_data->decelerate_after; - dt += (last_mm_remaining-mm_remaining)/st_prep_data->maximum_speed; - // current_speed = maximum_speed; - } else { // **Complete** Segment filled. - dt = DT_SEGMENT; - } - } - - // Fill incomplete segment with a deceleration junction. - if (mm_remaining > 0.0) { - if (mm_remaining <= st_prep_data->decelerate_after) { // Deceleration ramp - last_mm_remaining = mm_remaining; - dt_remainder = DT_SEGMENT-dt; - mm_remaining -= (dt_remainder*(st_prep_data->current_speed - - 0.5*pl_prep_block->acceleration*dt_remainder)); - if (mm_remaining > 0.0) { // **Complete** Segment filled. - st_prep_data->current_speed -= pl_prep_block->acceleration*dt_remainder; - dt = DT_SEGMENT; - } else { // **Complete** End of block. - mm_remaining = 0.0; - dt += (2*last_mm_remaining/(st_prep_data->current_speed+st_prep_data->exit_speed)); - // st_prep_data->current_speed = st_prep_data->exit_speed; - } - } - } - - } - } - - // ----------------------------------------------------------------------------------- - // Compute segment step rate, steps to execute, and step phase correction parameters. - - // Convert segment distance in terms of steps. -// float dist_travel = pl_prep_block->millimeters; -// if (mm_remaining > 0.0) { dist_travel -= mm_remaining; } - - - if (mm_remaining > 0.0) { - - float steps_remaining = st_prep_step_per_mm*mm_remaining; - prep_segment->dist_per_tick = ceil( (INV_TIME_MULTIPLIER/ISR_TICKS_PER_SECOND)* - (st_prep_data->step_events_remaining-steps_remaining)/dt ); // (mult*step/isr_tic) - - // Compute number of steps to execute and segment step phase correction. - prep_segment->n_step = ceil(st_prep_data->step_events_remaining)-ceil(steps_remaining); - prep_segment->n_phase_tick = ceil(INV_TIME_MULTIPLIER*(ceil(steps_remaining)-steps_remaining)/prep_segment->dist_per_tick); - - // Update step execution variables - st_prep_data->step_events_remaining = steps_remaining; - pl_prep_block->millimeters = mm_remaining; - - } else { // End of block. Finish it out. - - prep_segment->dist_per_tick = ceil( (INV_TIME_MULTIPLIER/ISR_TICKS_PER_SECOND)* - st_prep_data->step_events_remaining/dt ); // (mult*step/isr_tic) - - // Set to execute the remaining steps and no phase correction upon finishing the block. - prep_segment->n_step = ceil(st_prep_data->step_events_remaining); - prep_segment->n_phase_tick = 0; - - // NOTE: Not required. Planner will ignore this block as it is now complete. - // st_prep_data->step_events_remaining = 0.0; - // pl_prep_block->millimeters = 0.0; - - // Move planner pointer to next block and flag to load a new block for the next segment. - pl_prep_index = plan_next_block_index(pl_prep_index); - pl_prep_block = NULL; - prep_segment->flag |= SEGMENT_END_OF_BLOCK; - - } - - // !!! PROBLEM. Step events remaining in floating point can limit the number of steps - // we can accurately track, since floats have ~7.2 significant digits. However, this only - // becomes a problem if there are more than 1,000,000, which translates to a CNC machine - // with 200 step/mm and 5 meters of axis travel. Possible but unlikely. Could have more - // issues with user setting up their machine with too high of steps. - - // TODO: dist_per_tick must be less than INV_TIME_MULTIPLIER. A check can be made to - // make this a hard limit. Need to make sure this doesn't affect the velocity profiles.. - // it shouldn't. The same could said for the minimum allowable step rate too. This should - // not affect the tracing of the profiles either. - - // Ensure the initial step rate exceeds the MINIMUM_STEP_RATE. - // TODO: Use config.h error checking to do this. Otherwise, counters get screwy. - - // New step segment initialization completed. Increment segment buffer indices. - segment_buffer_head = segment_next_head; - if ( ++segment_next_head == SEGMENT_BUFFER_SIZE ) { segment_next_head = 0; } - - SPINDLE_ENABLE_PORT ^= 1<entry_speed_sqr = st_prep_data->current_speed*st_prep_data->current_speed; - // pl_partial_block->max_entry_speed_sqr = pl_partial_block->entry_speed_sqr; // Not sure if this needs to be updated. - - // Flag for new prep_block when st_prep_buffer() is called after the planner recomputes. - pl_partial_block_flag = true; - pl_prep_block = NULL; - } - return; -} From b562845d9d20bd09426f8d20647ed334eeb7491f Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Sat, 7 Dec 2013 20:08:24 -0700 Subject: [PATCH 25/73] Deceleration to zero speed improvements. Update defaults. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - A minor issue with deceleration ramps when close to zero velocity. Should be virtually unnoticeable for most CNC systems, but fixed in this push and accurate to physics. - Updated some of the junction deviation defaults. Because the new stepper algorithm can easily maximize a CNC machine’s capabilities or simply go much faster, this means the speed in which it enters junctions has to be a little more constrained. Meaning that, we have to slow a little bit down more so that we don’t exceed the acceleration limits of the stepper motors. --- defaults.h | 14 +++++----- stepper.c | 75 ++++++++++++++++++++++++++++++++---------------------- 2 files changed, 51 insertions(+), 38 deletions(-) diff --git a/defaults.h b/defaults.h index e94fcd1..d71ce09 100644 --- a/defaults.h +++ b/defaults.h @@ -45,7 +45,7 @@ #define DEFAULT_FEEDRATE 250.0 // mm/min #define DEFAULT_STEPPING_INVERT_MASK ((1<entry_speed_sqr); } } - prep.mm_eob = 0.0; - + /* --------------------------------------------------------------------------------- + Compute the velocity profile of a new planner block based on its entry and exit + speeds, or recompute the profile of a partially-completed planner block if the + planner has updated it. For a commanded forced-deceleration, such as from a feed + hold, override the planner velocities and decelerate to the target exit speed. + */ + prep.mm_complete = 0.0; float inv_2_accel = 0.5/pl_block->acceleration; if (sys.state == STATE_HOLD) { // Compute velocity profile parameters for a feed hold in-progress. This profile overrides @@ -501,12 +506,12 @@ void st_prep_buffer() float decel_dist = inv_2_accel*pl_block->entry_speed_sqr; if (decel_dist < pl_block->millimeters) { prep.exit_speed = 0.0; - prep.mm_eob = pl_block->millimeters-decel_dist; + prep.mm_complete = pl_block->millimeters-decel_dist; } else { prep.exit_speed = sqrt(pl_block->entry_speed_sqr-2*pl_block->acceleration*pl_block->millimeters); } } else { - // Compute velocity profile parameters of the prepped planner block. + // Compute or recompute velocity profile parameters of the prepped planner block. prep.ramp_type = RAMP_ACCEL; // Initialize as acceleration ramp. prep.accelerate_until = pl_block->millimeters; prep.exit_speed = plan_get_exec_block_exit_speed(); @@ -550,24 +555,31 @@ void st_prep_buffer() // Set new segment to point to the current segment data block. prep_segment->st_block_index = prep.st_block_index; - /* ----------------------------------------------------------------------------------- - Compute the average velocity of this new segment by determining the total distance - traveled over the segment time DT_SEGMENT. The following code first attempts to create - a full segment based on the current ramp conditions. If the segment time is incomplete - when terminating at a ramp state change, the code will continue to loop through the - progressing ramp states to fill the remaining segment execution time. However, if - an incomplete segment terminates at the end of the planner block, the segment is - considered completed despite having a truncated execution time less than DT_SEGMENT. + /*------------------------------------------------------------------------------------ + Compute the average velocity of this new segment by determining the total distance + traveled over the segment time DT_SEGMENT. The following code first attempts to create + a full segment based on the current ramp conditions. If the segment time is incomplete + when terminating at a ramp state change, the code will continue to loop through the + progressing ramp states to fill the remaining segment execution time. However, if + an incomplete segment terminates at the end of the velocity profile, the segment is + considered completed despite having a truncated execution time less than DT_SEGMENT. + The velocity profile is always assumed to progress through the ramp sequence: + acceleration ramp, cruising state, and deceleration ramp. Each ramp's travel distance + may range from zero to the length of the block. Velocity profiles can end either at + the end of planner block (typical) or mid-block at the end of a forced deceleration, + such as from a feed hold. */ float dt = 0.0; float mm_remaining = pl_block->millimeters; float time_var = DT_SEGMENT; // Time worker variable - float mm_var; // mm distance worker variable + float mm_var; // mm-Distance worker variable + float speed_var; // Speed work variable. do { switch (prep.ramp_type) { case RAMP_ACCEL: // NOTE: Acceleration ramp only computes during first do-while loop. - mm_remaining -= DT_SEGMENT*(prep.current_speed + pl_block->acceleration*(0.5*DT_SEGMENT)); + speed_var = pl_block->acceleration*DT_SEGMENT; + mm_remaining -= DT_SEGMENT*(prep.current_speed + 0.5*speed_var); if (mm_remaining < prep.accelerate_until) { // End of acceleration ramp. // Acceleration-cruise, acceleration-deceleration ramp junction, or end of block. mm_remaining = prep.accelerate_until; // NOTE: 0.0 at EOB @@ -576,7 +588,7 @@ void st_prep_buffer() else { prep.ramp_type = RAMP_CRUISE; } prep.current_speed = prep.maximum_speed; } else { // Acceleration only. - prep.current_speed += pl_block->acceleration*time_var; + prep.current_speed += speed_var; } break; case RAMP_CRUISE: @@ -592,23 +604,24 @@ void st_prep_buffer() } break; default: // case RAMP_DECEL: - // NOTE: mm_var used to catch negative decelerate distance values near zero speed. - mm_var = time_var*(prep.current_speed - 0.5*pl_block->acceleration*time_var); - if ((mm_var > prep.mm_eob) && (mm_var < mm_remaining)) { // Deceleration only. - prep.current_speed -= pl_block->acceleration*time_var; - // Check for near-zero speed and prevent divide by zero in rare scenarios. - if (prep.current_speed > prep.exit_speed) { mm_remaining -= mm_var; } - else { mm_remaining = prep.mm_eob; } // NOTE: Force EOB for now. May or may not be needed. - } else { // End of block. - time_var = 2.0*(mm_remaining-prep.mm_eob)/(prep.current_speed+prep.exit_speed); - mm_remaining = prep.mm_eob; - // prep.current_speed = prep.exit_speed; // !! May be needed for feed hold reinitialization. - } + // NOTE: mm_var used as a misc worker variable to prevent errors when near zero speed. + speed_var = pl_block->acceleration*time_var; // Used as delta speed (mm/min) + if (prep.current_speed > speed_var) { // Check if at or below zero speed. + // Compute distance from end of segment to end of block. + mm_var = mm_remaining - time_var*(prep.current_speed - 0.5*speed_var); // (mm) + if (mm_var > prep.mm_complete) { // Deceleration only. + mm_remaining = mm_var; + prep.current_speed -= speed_var; + break; // Segment complete. Exit switch-case statement. + } + } // End of block or end of forced-deceleration. + time_var = 2.0*(mm_remaining-prep.mm_complete)/(prep.current_speed+prep.exit_speed); + mm_remaining = prep.mm_complete; } dt += time_var; // Add computed ramp time to total segment time. if (dt < DT_SEGMENT) { time_var = DT_SEGMENT - dt; } // **Incomplete** At ramp junction. else { break; } // **Complete** Exit loop. Segment execution time maxed. - } while (mm_remaining > prep.mm_eob); // **Complete** Exit loop. End of planner block. + } while (mm_remaining > prep.mm_complete); // **Complete** Exit loop. Profile complete. /* ----------------------------------------------------------------------------------- Compute segment step rate, steps to execute, and step phase correction parameters. @@ -622,7 +635,7 @@ void st_prep_buffer() */ // Use time_var to pre-compute dt inversion with integer multiplier. time_var = (INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))/dt; // (mult/isr_tic) - if (mm_remaining > 0.0) { + if (mm_remaining > 0.0) { // Block still incomplete. Distance remaining to be executed. float steps_remaining = prep.step_per_mm*mm_remaining; prep_segment->dist_per_tick = ceil( (prep.steps_remaining-steps_remaining)*time_var ); // (mult*step/isr_tic) @@ -631,7 +644,7 @@ void st_prep_buffer() prep_segment->n_step = ceil(prep.steps_remaining)-ceil(steps_remaining); // Update step execution variables. - if (mm_remaining == prep.mm_eob) { + if (mm_remaining == prep.mm_complete) { // NOTE: Currently only feed holds qualify for this scenario. May change with overrides. prep.current_speed = 0.0; prep.steps_remaining = ceil(steps_remaining); From 3054b2df774c8baa19d56421466c0f3c44c00041 Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Tue, 10 Dec 2013 22:33:06 -0700 Subject: [PATCH 26/73] Revamped homing cycle. Axis limits and max travel bug fixes. Build info. Refactored config.h. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Revamped and improved homing cycle. Now tied directly into the main planner and stepper code, which enables much faster homing seek rates. Also dropped the compiled flash size by almost 1KB, meaning 1KB more for other features. - Refactored config.h. Removed obsolete defines and configuration options. Moved lots of “advanced” options into the advanced area of the file. - Updated defaults.h with the new homing cycle. Also updated the Sherline 5400 defaults and added the ShapeOko2 defaults per user submissions. - Fixed a bug where the individual axes limits on velocity and acceleration were not working correctly. Caused by abs() returning a int, rather than a float. Corrected with fabs(). Duh. - Added build version/date to the Grbl welcome message to help indicate which version a user is operating on. - Max travel settings were not being defaulted into the settings EEPROM correctly. Fixed. - To stop a single axis during a multi-axes homing move, the stepper algorithm now has a simple axis lock mask which inhibits the desired axes from moving. Meaning, if one of the limit switches engages before the other, we stop that one axes and keep moving the other. --- config.h | 117 +++++++++++++-------------------- defaults.h | 75 ++++++++++++++++----- limits.c | 181 +++++++++++++-------------------------------------- nuts_bolts.h | 1 + planner.c | 5 +- report.c | 2 +- report.h | 1 - settings.c | 6 +- settings.h | 3 +- stepper.c | 48 +++++++++----- 10 files changed, 188 insertions(+), 251 deletions(-) diff --git a/config.h b/config.h index 614fcd1..9df2677 100644 --- a/config.h +++ b/config.h @@ -29,7 +29,7 @@ #define config_h // Default settings. Used when resetting EEPROM. Change to desired name in defaults.h -#define DEFAULTS_ZEN_TOOLWORKS_7x7 +#define DEFAULTS_SHERLINE_5400 // Serial baud rate #define BAUD_RATE 115200 @@ -49,76 +49,17 @@ #define CMD_CYCLE_START '~' #define CMD_RESET 0x18 // ctrl-x -// The "Stepper Driver Interrupt" employs an inverse time algorithm to manage the Bresenham line -// stepping algorithm. The value ISR_TICKS_PER_SECOND is the frequency(Hz) at which the inverse time -// algorithm ticks at. Recommended step frequencies are limited by the inverse time frequency by -// approximately 0.75-0.9 * ISR_TICK_PER_SECOND. Meaning for 30kHz, the max step frequency is roughly -// 22.5-27kHz, but 30kHz is still possible, just not optimal. An Arduino can safely complete a single -// interrupt of the current stepper driver algorithm theoretically up to a frequency of 35-40kHz, but -// CPU overhead increases exponentially as this frequency goes up. So there will be little left for -// other processes like arcs. -#define ISR_TICKS_PER_SECOND 30000L // 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. -// NOTE: Ramp count variable type in stepper module may need to be updated if changed. -#define ACCELERATION_TICKS_PER_SECOND 120L - -// NOTE: Make sure this value is less than 256, when adjusting both dependent parameters. -#define ISR_TICKS_PER_ACCELERATION_TICK (ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND) - -// The inverse time algorithm can use either floating point or long integers for its counters (usually -// very small values ~10^-6), but with integers, the counter values must be scaled to be greater than -// one. This multiplier value scales the floating point counter values for use in a long integer, which -// are significantly faster to compute with a slightly higher precision ceiling than floats. Long -// integers are finite so select the multiplier value high enough to avoid any numerical round-off -// issues and still have enough range to account for all motion types. However, in most all imaginable -// CNC applications, the following multiplier value will work more than well enough. If you do have -// happened to weird stepper motion issues, try modifying this value by adding or subtracting a -// zero and report it to the Grbl administrators. -#define INV_TIME_MULTIPLIER 100000 - -// Minimum stepper rate for the "Stepper Driver Interrupt". Sets the absolute minimum stepper rate -// in the stepper program and never runs slower than this value. If the INVE_TIME_MULTIPLIER value -// changes, it will affect how this value works. So, if a zero is add/subtracted from the -// INV_TIME_MULTIPLIER value, do the same to this value if you want to same response. -// NOTE: Compute by (desired_step_rate/60) * INV_TIME_MULTIPLIER/ISR_TICKS_PER_SECOND. (mm/min) -// #define MINIMUM_STEP_RATE 1000L // Integer (mult*mm/isr_tic) - -// Minimum stepper rate. Only used by homing at this point. May be removed in later releases. -#define MINIMUM_STEPS_PER_MINUTE 800 // (steps/min) - Integer value only - -// Minimum planner junction speed. Sets the default minimum junction speed the planner plans to at -// every buffer block junction, except for starting from rest and end of the buffer, which are always -// zero. This value controls how fast the machine moves through junctions with no regard for acceleration -// limits or angle between neighboring block line move directions. This is useful for machines that can't -// tolerate the tool dwelling for a split second, i.e. 3d printers or laser cutters. If used, this value -// should not be much greater than zero or to the minimum value necessary for the machine to work. -#define MINIMUM_JUNCTION_SPEED 0.0 // (mm/min) - -// Time delay increments performed during a dwell. The default value is set at 50ms, which provides -// a maximum time delay of roughly 55 minutes, more than enough for most any application. Increasing -// this delay will increase the maximum dwell time linearly, but also reduces the responsiveness of -// run-time command executions, like status reports, since these are performed between each dwell -// time step. Also, keep in mind that the Arduino delay timer is not very accurate for long delays. -#define DWELL_TIME_STEP 50 // Integer (1-255) (milliseconds) +// Uncomment the following define if you are using hardware that drives high when your limits +// are reached. You will need to ensure that you have appropriate pull-down resistors on the +// limit switch input pins, or that your hardware drives the pins low when they are open (non- +// triggered). +// #define LIMIT_SWITCHES_ACTIVE_HIGH // If homing is enabled, homing init lock sets Grbl into an alarm state upon power up. This forces // the user to perform the homing cycle (or override the locks) before doing anything else. This is // mainly a safety feature to remind the user to home, since position is unknown to Grbl. #define HOMING_INIT_LOCK // Comment to disable -// The homing cycle seek and feed rates will adjust so all axes independently move at the homing -// seek and feed rates regardless of how many axes are in motion simultaneously. If disabled, rates -// are point-to-point rates, as done in normal operation. For example in an XY diagonal motion, the -// diagonal motion moves at the intended rate, but the individual axes move at 70% speed. This option -// just moves them all at 100% speed. -#define HOMING_RATE_ADJUST // Comment to disable - // Define the homing cycle search patterns with bitmasks. The homing cycle first performs a search // to engage the limit switches. HOMING_SEARCH_CYCLE_x are executed in order starting with suffix 0 // and searches the enabled axes in the bitmask. This allows for users with non-standard cartesian @@ -144,6 +85,39 @@ // parser state depending on user preferences. #define N_STARTUP_LINE 2 // Integer (1-5) +// --------------------------------------------------------------------------------------- +// ADVANCED CONFIGURATION OPTIONS: + +// The "Stepper Driver Interrupt" employs an inverse time algorithm to manage the Bresenham line +// stepping algorithm. The value ISR_TICKS_PER_SECOND is the frequency(Hz) at which the inverse time +// algorithm ticks at. Recommended step frequencies are limited by the inverse time frequency by +// approximately 0.75-0.9 * ISR_TICK_PER_SECOND. Meaning for 30kHz, the max step frequency is roughly +// 22.5-27kHz, but 30kHz is still possible, just not optimal. An Arduino can safely complete a single +// interrupt of the current stepper driver algorithm theoretically up to a frequency of 35-40kHz, but +// CPU overhead increases exponentially as this frequency goes up. So there will be little left for +// other processes like arcs. +#define ISR_TICKS_PER_SECOND 30000L // 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. +// NOTE: Ramp count variable type in stepper module may need to be updated if changed. +#define ACCELERATION_TICKS_PER_SECOND 120L + +// NOTE: Make sure this value is less than 256, when adjusting both dependent parameters. +#define ISR_TICKS_PER_ACCELERATION_TICK (ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND) + +// Minimum planner junction speed. Sets the default minimum junction speed the planner plans to at +// every buffer block junction, except for starting from rest and end of the buffer, which are always +// zero. This value controls how fast the machine moves through junctions with no regard for acceleration +// limits or angle between neighboring block line move directions. This is useful for machines that can't +// tolerate the tool dwelling for a split second, i.e. 3d printers or laser cutters. If used, this value +// should not be much greater than zero or to the minimum value necessary for the machine to work. +#define MINIMUM_JUNCTION_SPEED 0.0 // (mm/min) + // 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 @@ -151,8 +125,12 @@ // computational efficiency of generating arcs. #define N_ARC_CORRECTION 20 // Integer (1-255) -// --------------------------------------------------------------------------------------- -// FOR ADVANCED USERS ONLY: +// Time delay increments performed during a dwell. The default value is set at 50ms, which provides +// a maximum time delay of roughly 55 minutes, more than enough for most any application. Increasing +// this delay will increase the maximum dwell time linearly, but also reduces the responsiveness of +// run-time command executions, like status reports, since these are performed between each dwell +// time step. Also, keep in mind that the Arduino delay timer is not very accurate for long delays. +#define DWELL_TIME_STEP 50 // Integer (1-255) (milliseconds) // The number of linear motions in the planner buffer to be planned at any give time. The vast // majority of RAM that Grbl uses is based on this buffer size. Only increase if there is extra @@ -198,11 +176,6 @@ // case, please report any successes to grbl administrators! // #define ENABLE_XONXOFF // Default disabled. Uncomment to enable. -// Uncomment the following define if you are using hardware that drives high when your limits -// are reached. You will need to ensure that you have appropriate pull-down resistors on the -// limit switch input pins, or that your hardware drives the pins low when they are open (non- -// triggered). -// #define LIMIT_SWITCHES_ACTIVE_HIGH // --------------------------------------------------------------------------------------- diff --git a/defaults.h b/defaults.h index d71ce09..bfbb88e 100644 --- a/defaults.h +++ b/defaults.h @@ -56,8 +56,8 @@ #define DEFAULT_HOMING_ENABLE 0 // false #define DEFAULT_HOMING_DIR_MASK 0 // move positive dir #define DEFAULT_HOMING_FEED_RATE 25.0 // mm/min - #define DEFAULT_HOMING_SEEK_RATE 250.0 // mm/min - #define DEFAULT_HOMING_DEBOUNCE_DELAY 100 // msec (0-65k) + #define DEFAULT_HOMING_SEEK_RATE 500.0 // mm/min + #define DEFAULT_HOMING_DEBOUNCE_DELAY 250 // msec (0-65k) #define DEFAULT_HOMING_PULLOFF 1.0 // mm #endif @@ -76,9 +76,9 @@ #define DEFAULT_X_ACCELERATION (50.0*60*60) // 10 mm/min^2 #define DEFAULT_Y_ACCELERATION (50.0*60*60) // 10 mm/min^2 #define DEFAULT_Z_ACCELERATION (50.0*60*60) // 10 mm/min^2 - #define DEFAULT_X_MAX_TRAVEL 200.0 // mm - #define DEFAULT_Y_MAX_TRAVEL 200.0 // mm - #define DEFAULT_Z_MAX_TRAVEL 200.0 // mm + #define DEFAULT_X_MAX_TRAVEL 225.0 // mm + #define DEFAULT_Y_MAX_TRAVEL 125.0 // mm + #define DEFAULT_Z_MAX_TRAVEL 170.0 // mm #define DEFAULT_STEP_PULSE_MICROSECONDS 10 #define DEFAULT_FEEDRATE 254.0 // mm/min (10 ipm) #define DEFAULT_STEPPING_INVERT_MASK ((1< dt_min) { dt = dt_min; } // Disable acceleration for very slow rates. - - // Set default out_bits. - uint8_t out_bits0 = settings.invert_mask; - out_bits0 ^= (settings.homing_dir_mask & DIRECTION_MASK); // Apply homing direction settings - if (!pos_dir) { out_bits0 ^= DIRECTION_MASK; } // Invert bits, if negative dir. - - // Initialize stepping variables - int32_t counter_x = -(step_event_count >> 1); // Bresenham counters - int32_t counter_y = counter_x; - int32_t counter_z = counter_x; - uint32_t step_delay = dt-settings.pulse_microseconds; // Step delay after pulse - uint32_t step_rate = 0; // Tracks step rate. Initialized from 0 rate. (in step/min) - uint32_t trap_counter = MICROSECONDS_PER_ACCELERATION_TICK/2; // Acceleration trapezoid counter - uint8_t out_bits; + if (sys.execute & EXEC_RESET) { return; } uint8_t limit_state; - for(;;) { + #ifndef LIMIT_SWITCHES_ACTIVE_HIGH + invert_pin = !invert_pin; + #endif + + // Compute target location for homing all axes. Homing axis lock will freeze non-cycle axes. + float target[N_AXIS]; + target[X_AXIS] = settings.max_travel[X_AXIS]; + if (target[X_AXIS] < settings.max_travel[Y_AXIS]) { target[X_AXIS] = settings.max_travel[Y_AXIS]; } + if (target[X_AXIS] < settings.max_travel[Z_AXIS]) { target[X_AXIS] = settings.max_travel[Z_AXIS]; } + target[X_AXIS] *= 2.0; + if (pos_dir) { target[X_AXIS] = -target[X_AXIS]; } + target[Y_AXIS] = target[X_AXIS]; + target[Z_AXIS] = target[X_AXIS]; + homing_rate *= 1.7320; // [sqrt(N_AXIS)] Adjust so individual axes all move at homing rate. + + // Setup homing axis locks based on cycle mask. + uint8_t axislock = (STEPPING_MASK & ~STEP_MASK); + if (bit_istrue(cycle_mask,bit(X_AXIS))) { axislock |= (1< 0) { - if (limit_state & (1< 0) { - if (limit_state & (1< 0) { - if (limit_state & (1< dt_min) { // Unless cruising, check for time update. - trap_counter += dt; // Track time passed since last update. - if (trap_counter > MICROSECONDS_PER_ACCELERATION_TICK) { - trap_counter -= MICROSECONDS_PER_ACCELERATION_TICK; - step_rate += delta_rate; // Increment velocity - dt = (1000000*60)/step_rate; // Compute new time increment - if (dt < dt_min) {dt = dt_min;} // If target rate reached, cruise. - step_delay = dt-settings.pulse_microseconds; - } + if (axislock & (1< 0) { // Re-approach all switches to re-engage them. homing_cycle(HOMING_LOCATE_CYCLE, true, false, settings.homing_feed_rate); - delay_ms(settings.homing_debounce_delay); } } - - st_go_idle(); // Call main stepper shutdown routine. } diff --git a/nuts_bolts.h b/nuts_bolts.h index 3c5c951..59aeff6 100644 --- a/nuts_bolts.h +++ b/nuts_bolts.h @@ -89,6 +89,7 @@ typedef struct { uint8_t abort; // System abort flag. Forces exit back to main loop for reset. uint8_t state; // Tracks the current state of Grbl. volatile uint8_t execute; // Global system runtime executor bitflag variable. See EXEC bitmasks. + uint8_t homing_axis_lock; int32_t position[N_AXIS]; // Real-time machine (aka home) position vector in steps. // NOTE: This may need to be a volatile variable, if problems arise. uint8_t auto_start; // Planner auto-start flag. Toggled off during feed hold. Defaulted by settings. diff --git a/planner.c b/planner.c index 1fc8e1a..d4b4326 100644 --- a/planner.c +++ b/planner.c @@ -326,7 +326,7 @@ void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) for (idx=0; idxdist_per_tick; // Execute Bresenham step event, when it's time to do so. - if (st.counter_dist > INV_TIME_MULTIPLIER) { + if (st.counter_dist > STEP_FTOL_MULTIPLIER) { if (st.step_count > 0) { // Block phase correction from executing step. - st.counter_dist -= INV_TIME_MULTIPLIER; // Reload inverse time counter + st.counter_dist -= STEP_FTOL_MULTIPLIER; // Reload inverse time counter st.step_count--; // Decrement step events count // Execute step displacement profile by Bresenham line algorithm st.execute_step = true; - st.out_bits = st.exec_block->direction_bits; // Reset out_bits and reload direction bits + st.out_bits = st.exec_block->direction_bits; // Reset out_bits and reload direction bits st.counter_x -= st.exec_block->steps[X_AXIS]; if (st.counter_x < 0) { st.out_bits |= (1<step_event_count; prep.step_per_mm = prep.steps_remaining/pl_block->millimeters; - if (sys.state == STATE_HOLD) { + if (sys.state == STATE_HOLD) { + // Override planner block entry speed and enforce deceleration during feed hold. prep.current_speed = prep.exit_speed; pl_block->entry_speed_sqr = prep.exit_speed*prep.exit_speed; } @@ -497,7 +503,7 @@ void st_prep_buffer() planner has updated it. For a commanded forced-deceleration, such as from a feed hold, override the planner velocities and decelerate to the target exit speed. */ - prep.mm_complete = 0.0; + prep.mm_complete = 0.0; // Default velocity profile complete at 0.0mm from end of block. float inv_2_accel = 0.5/pl_block->acceleration; if (sys.state == STATE_HOLD) { // Compute velocity profile parameters for a feed hold in-progress. This profile overrides @@ -506,7 +512,7 @@ void st_prep_buffer() float decel_dist = inv_2_accel*pl_block->entry_speed_sqr; if (decel_dist < pl_block->millimeters) { prep.exit_speed = 0.0; - prep.mm_complete = pl_block->millimeters-decel_dist; + prep.mm_complete = pl_block->millimeters-decel_dist; // End of feed hold. } else { prep.exit_speed = sqrt(pl_block->entry_speed_sqr-2*pl_block->acceleration*pl_block->millimeters); } @@ -634,13 +640,13 @@ void st_prep_buffer() supported by Grbl (i.e. exceeding 10 meters axis travel at 200 step/mm). */ // Use time_var to pre-compute dt inversion with integer multiplier. - time_var = (INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))/dt; // (mult/isr_tic) + time_var = (STEP_FTOL_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))/dt; // (ftol_mult/isr_tic) if (mm_remaining > 0.0) { // Block still incomplete. Distance remaining to be executed. float steps_remaining = prep.step_per_mm*mm_remaining; - prep_segment->dist_per_tick = ceil( (prep.steps_remaining-steps_remaining)*time_var ); // (mult*step/isr_tic) + prep_segment->dist_per_tick = ceil( (prep.steps_remaining-steps_remaining)*time_var ); // (ftol_mult*step/isr_tic) // Compute number of steps to execute and segment step phase correction. - prep_segment->phase_dist = ceil(INV_TIME_MULTIPLIER*(ceil(steps_remaining)-steps_remaining)); + prep_segment->phase_dist = ceil(STEP_FTOL_MULTIPLIER*(ceil(steps_remaining)-steps_remaining)); prep_segment->n_step = ceil(prep.steps_remaining)-ceil(steps_remaining); // Update step execution variables. @@ -648,7 +654,7 @@ void st_prep_buffer() // NOTE: Currently only feed holds qualify for this scenario. May change with overrides. prep.current_speed = 0.0; prep.steps_remaining = ceil(steps_remaining); - pl_block->millimeters = prep.steps_remaining/prep.step_per_mm; + pl_block->millimeters = prep.steps_remaining/prep.step_per_mm; // Update with full steps. plan_cycle_reinitialize(); sys.state = STATE_QUEUED; // End cycle. } else { @@ -682,12 +688,14 @@ void st_prep_buffer() // int32_t blength = segment_buffer_head - segment_buffer_tail; // if (blength < 0) { blength += SEGMENT_BUFFER_SIZE; } // printInteger(blength); + + if ((sys.state == STATE_HOMING) || (sys.state == STATE_QUEUED)) { return; } // Force only one prepped segment. } } /* TODO: With feedrate overrides, increases to the override value will not significantly - change the planner and stepper current operation. When the value increases, we simply + change the current planner and stepper operation. When the value increases, we simply need to recompute the block plan with new nominal speeds and maximum junction velocities. However with a decreasing feedrate override, this gets a little tricky. The current block plan is optimal, so if we try to reduce the feed rates, it may be impossible to create @@ -704,4 +712,10 @@ void st_prep_buffer() equal to the block maximum speed and is in an acceleration or cruising ramp. At this point, we know that we can recompute the block velocity profile to meet and continue onto the new block plan. + One "easy" way to do this is to have the step segment buffer enforce a deceleration and + continually re-plan the planner buffer until the plan becomes feasible. This can work + and may be easy to implement, but it expends a lot of CPU cycles and may block out the + rest of the functions from operating at peak efficiency. Still the question is how do + we know when the plan is feasible in the context of what's already in the code and not + require too much more code? */ From eefc25be91615c7533bd95ea3d57ae8642759dad Mon Sep 17 00:00:00 2001 From: Scott R Carlson Date: Fri, 27 Dec 2013 15:09:00 -0500 Subject: [PATCH 27/73] Hard Limits configured for active high. Added the use of homing_dir_mask to homing_cycle --- config.h | 2 +- limits.c | 30 ++++++++++++++++++++++++++---- 2 files changed, 27 insertions(+), 5 deletions(-) diff --git a/config.h b/config.h index 9df2677..1fce110 100644 --- a/config.h +++ b/config.h @@ -53,7 +53,7 @@ // are reached. You will need to ensure that you have appropriate pull-down resistors on the // limit switch input pins, or that your hardware drives the pins low when they are open (non- // triggered). -// #define LIMIT_SWITCHES_ACTIVE_HIGH +#define LIMIT_SWITCHES_ACTIVE_HIGH // If homing is enabled, homing init lock sets Grbl into an alarm state upon power up. This forces // the user to perform the homing cycle (or override the locks) before doing anything else. This is diff --git a/limits.c b/limits.c index a9a7e29..f91cf56 100644 --- a/limits.c +++ b/limits.c @@ -87,7 +87,7 @@ ISR(LIMIT_INT_vect) // algorithm is written here. This also lets users hack and tune this code freely for // their own particular needs without affecting the rest of Grbl. // NOTE: Only the abort runtime command can interrupt this process. -static void homing_cycle(uint8_t cycle_mask, bool pos_dir, bool invert_pin, float homing_rate) +static void homing_cycle(uint8_t cycle_mask, bool approach, bool invert_pin, float homing_rate) { if (sys.execute & EXEC_RESET) { return; } uint8_t limit_state; @@ -101,9 +101,31 @@ static void homing_cycle(uint8_t cycle_mask, bool pos_dir, bool invert_pin, floa if (target[X_AXIS] < settings.max_travel[Y_AXIS]) { target[X_AXIS] = settings.max_travel[Y_AXIS]; } if (target[X_AXIS] < settings.max_travel[Z_AXIS]) { target[X_AXIS] = settings.max_travel[Z_AXIS]; } target[X_AXIS] *= 2.0; - if (pos_dir) { target[X_AXIS] = -target[X_AXIS]; } - target[Y_AXIS] = target[X_AXIS]; - target[Z_AXIS] = target[X_AXIS]; + uint8_t pos_dir = settings.homing_dir_mask; + if (pos_dir >= 128) { + pos_dir = pos_dir - 128; + target[Z_AXIS] = target[X_AXIS]; + } + else { target[Z_AXIS] = -target[X_AXIS]; } + if (!approach) { target[Z_AXIS] = target[Z_AXIS] * -1; } + + if (pos_dir >= 64) { + pos_dir = pos_dir - 64; + target[Y_AXIS] = target[X_AXIS]; + } + else { target[Y_AXIS] = -target[X_AXIS]; } + if (!approach) { target[Y_AXIS] = target[Y_AXIS] * -1; } + +if (pos_dir >= 32) { + pos_dir = pos_dir -32; + target[X_AXIS] = target[X_AXIS]; + } + else { target[X_AXIS] = -target[X_AXIS]; } + if (!approach) { target[X_AXIS] = target[X_AXIS] * -1; } + + //if (pos_dir) { target[X_AXIS] = -target[X_AXIS]; } + //target[Y_AXIS] = target[X_AXIS]; + //target[Z_AXIS] = target[X_AXIS]; homing_rate *= 1.7320; // [sqrt(N_AXIS)] Adjust so individual axes all move at homing rate. // Setup homing axis locks based on cycle mask. From 903b462b2e37b18a8f7884924109fecd66c69e27 Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Sun, 29 Dec 2013 20:34:51 -0700 Subject: [PATCH 28/73] Merge branch 'dev' of https://github.com/grbl/grbl into dev Conflicts: limits.c --- README.md | 2 +- config.h | 48 +++--- limits.c | 168 +++++++++++--------- limits.h | 6 +- main.c | 30 ++-- motion_control.c | 42 ++++- motion_control.h | 2 +- nuts_bolts.h | 17 +-- planner.c | 4 +- planner.h | 8 +- protocol.c | 4 +- report.c | 1 - stepper.c | 390 +++++++++++++++++++++++++++-------------------- stepper.h | 4 +- 14 files changed, 412 insertions(+), 314 deletions(-) diff --git a/README.md b/README.md index e29a19e..c58f3a1 100644 --- a/README.md +++ b/README.md @@ -24,7 +24,7 @@ Grbl includes full acceleration management with look ahead. That means the contr - Soft limits: Checks if any motion command exceeds workspace limits. Alarms out when found. Another safety feature, but, unlike hard limits, position does not get lost, as it forces a feed hold before erroring out. - Pin mapping: In an effort for Grbl to be compatible with other AVR architectures, such as the 1280 or 2560, a new pin_map.h configuration file has been created to allow Grbl to be compiled for them. This is currently user supported, so your mileage may vary. If you run across a bug, please let us know or better send us a fix! Thanks in advance! - New Grbl SIMULATOR by @jgeisler: A completely independent wrapper of the Grbl main source code that may be compiled as an executable on a computer. No Arduino required. Simply simulates the responses of Grbl as if it was on an Arduino. May be used for many things: checking out how Grbl works, pre-process moves for GUI graphics, debugging of new features, etc. Much left to do, but potentially very powerful, as the dummy AVR variables can be written to output anything you need. - - Homing routine updated: Sets workspace volume in all negative space regardless of limit switch position. Common on pro CNCs. Also reduces soft limits CPU overhead. + - Homing routine updated: Sets workspace volume in all negative space regardless of limit switch position. Common on pro CNCs. Now tied directly into the main planner and stepper modules to reduce flash space and allow maximum speeds during seeking. - Feedrate overrides: In the works, but planner has begun to be re-factored for this feature. - Jogging controls: Methodology needs to be to figured out first. Could be dropped due to flash space concerns. Last item on the agenda. diff --git a/config.h b/config.h index 9df2677..aad11c3 100644 --- a/config.h +++ b/config.h @@ -47,13 +47,13 @@ #define CMD_STATUS_REPORT '?' #define CMD_FEED_HOLD '!' #define CMD_CYCLE_START '~' -#define CMD_RESET 0x18 // ctrl-x +#define CMD_RESET 0x18 // ctrl-x. // Uncomment the following define if you are using hardware that drives high when your limits // are reached. You will need to ensure that you have appropriate pull-down resistors on the // limit switch input pins, or that your hardware drives the pins low when they are open (non- // triggered). -// #define LIMIT_SWITCHES_ACTIVE_HIGH +// #define LIMIT_SWITCHES_ACTIVE_HIGH // Uncomment to enable // If homing is enabled, homing init lock sets Grbl into an alarm state upon power up. This forces // the user to perform the homing cycle (or override the locks) before doing anything else. This is @@ -88,27 +88,22 @@ // --------------------------------------------------------------------------------------- // ADVANCED CONFIGURATION OPTIONS: -// The "Stepper Driver Interrupt" employs an inverse time algorithm to manage the Bresenham line -// stepping algorithm. The value ISR_TICKS_PER_SECOND is the frequency(Hz) at which the inverse time -// algorithm ticks at. Recommended step frequencies are limited by the inverse time frequency by -// approximately 0.75-0.9 * ISR_TICK_PER_SECOND. Meaning for 30kHz, the max step frequency is roughly -// 22.5-27kHz, but 30kHz is still possible, just not optimal. An Arduino can safely complete a single -// interrupt of the current stepper driver algorithm theoretically up to a frequency of 35-40kHz, but -// CPU overhead increases exponentially as this frequency goes up. So there will be little left for -// other processes like arcs. -#define ISR_TICKS_PER_SECOND 30000L // Integer (Hz) +// The temporal resolution of the acceleration management subsystem. A higher number gives smoother +// acceleration, particularly noticeable on machines that run at very high feedrates, but may negatively +// impact performance. 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. +#define ACCELERATION_TICKS_PER_SECOND 100 -// 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. -// NOTE: Ramp count variable type in stepper module may need to be updated if changed. -#define ACCELERATION_TICKS_PER_SECOND 120L - -// NOTE: Make sure this value is less than 256, when adjusting both dependent parameters. -#define ISR_TICKS_PER_ACCELERATION_TICK (ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND) +// Creates a delay between the direction pin setting and corresponding step pulse by creating +// another interrupt (Timer2 compare) to manage it. The main Grbl interrupt (Timer1 compare) +// sets the direction pins, and does not immediately set the stepper pins, as it would in +// normal operation. The Timer2 compare fires next to set the stepper pins after the step +// pulse delay time, and Timer2 overflow will complete the step pulse, except now delayed +// by the step pulse time plus the step pulse delay. (Thanks langwadt for the idea!) +// NOTE: Uncomment to enable. The recommended delay must be > 3us, and, when added with the +// user-supplied step pulse time, the total time must not exceed 127us. Reported successful +// values for certain setups have ranged from 5 to 20us. +// #define STEP_PULSE_DELAY 10 // Step pulse delay in microseconds. Default disabled. // Minimum planner junction speed. Sets the default minimum junction speed the planner plans to at // every buffer block junction, except for starting from rest and end of the buffer, which are always @@ -145,7 +140,7 @@ // block velocity profile is traced exactly. The size of this buffer governs how much step // execution lead time there is for other Grbl processes have to compute and do their thing // before having to come back and refill this buffer, currently at ~50msec of step moves. -// #define SEGMENT_BUFFER_SIZE 7 // Uncomment to override default in stepper.h. +// #define SEGMENT_BUFFER_SIZE 6 // Uncomment to override default in stepper.h. // Line buffer size from the serial input stream to be executed. Also, governs the size of // each of the startup blocks, as they are each stored as a string of this size. Make sure @@ -176,6 +171,7 @@ // case, please report any successes to grbl administrators! // #define ENABLE_XONXOFF // Default disabled. Uncomment to enable. +#define ENABLE_SOFTWARE_DEBOUNCE // --------------------------------------------------------------------------------------- @@ -184,9 +180,9 @@ // --------------------------------------------------------------------------------------- // COMPILE-TIME ERROR CHECKING OF DEFINE VALUES: -#if (ISR_TICKS_PER_ACCELERATION_TICK > 255) -#error Parameters ACCELERATION_TICKS / ISR_TICKS must be < 256 to prevent integer overflow. -#endif +// #if (ISR_TICKS_PER_ACCELERATION_TICK > 255) +// #error Parameters ACCELERATION_TICKS / ISR_TICKS must be < 256 to prevent integer overflow. +// #endif // --------------------------------------------------------------------------------------- #endif diff --git a/limits.c b/limits.c index a9a7e29..a403f3a 100644 --- a/limits.c +++ b/limits.c @@ -22,6 +22,7 @@ #include #include #include +#include #include "stepper.h" #include "settings.h" #include "nuts_bolts.h" @@ -40,17 +41,29 @@ void limits_init() #ifndef LIMIT_SWITCHES_ACTIVE_HIGH LIMIT_PORT |= (LIMIT_MASK); // Enable internal pull-up resistors. Normal high operation. - #else // LIMIT_SWITCHES_ACTIVE_HIGH + #else LIMIT_PORT &= ~(LIMIT_MASK); // Normal low operation. Requires external pull-down. - #endif // !LIMIT_SWITCHES_ACTIVE_HIGH + #endif if (bit_istrue(settings.flags,BITFLAG_HARD_LIMIT_ENABLE)) { LIMIT_PCMSK |= LIMIT_MASK; // Enable specific pins of the Pin Change Interrupt PCICR |= (1 << LIMIT_INT); // Enable Pin Change Interrupt } else { - LIMIT_PCMSK &= ~LIMIT_MASK; // Disable - PCICR &= ~(1 << LIMIT_INT); + limits_disable(); } + + #ifdef ENABLE_SOFTWARE_DEBOUNCE + MCUSR &= ~(1< 0) { - // Re-approach all switches to re-engage them. - homing_cycle(HOMING_LOCATE_CYCLE, true, false, settings.homing_feed_rate); - } - } + delay_ms(settings.homing_debounce_delay); // Delay to allow transient dynamics to dissipate. } diff --git a/limits.h b/limits.h index ac94dd6..52881dc 100644 --- a/limits.h +++ b/limits.h @@ -25,8 +25,10 @@ // Initialize the limits module void limits_init(); -// Perform the homing cycle -void limits_go_home(); +void limits_disable(); + +// Perform one portion of the homing cycle based on the input settings. +void limits_go_home(uint8_t cycle_mask, bool approach, bool invert_pin, float homing_rate); // Check for soft limit violations void limits_soft_check(float *target); diff --git a/main.c b/main.c index 9decae8..0fc5a67 100644 --- a/main.c +++ b/main.c @@ -24,7 +24,6 @@ as being a consistent sounding board for the future of accessible and free CNC. */ #include -#include #include "config.h" #include "planner.h" #include "nuts_bolts.h" @@ -52,7 +51,17 @@ int main(void) memset(&sys, 0, sizeof(sys)); // Clear all system variables sys.abort = true; // Set abort to complete initialization - sys.state = STATE_INIT; // Set alarm state to indicate unknown initial position + + // Check for power-up and set system alarm if homing is enabled to force homing cycle + // by setting Grbl's alarm state. Alarm locks out all g-code commands, including the + // startup scripts, but allows access to settings and internal commands. Only a homing + // cycle '$H' or kill alarm locks '$X' will disable the alarm. + // NOTE: The startup script will run after successful completion of the homing cycle, but + // not after disabling the alarm locks. Prevents motion startup blocks from crashing into + // things uncontrollably. Very bad. + #ifdef HOMING_INIT_LOCK + if (bit_istrue(settings.flags,BITFLAG_HOMING_ENABLE)) { sys.state = STATE_ALARM; } + #endif for(;;) { @@ -62,12 +71,12 @@ int main(void) if (sys.abort) { // Reset system. serial_reset_read_buffer(); // Clear serial read buffer - plan_init(); // Clear block buffer and planner variables gc_init(); // Set g-code parser to default state protocol_init(); // Clear incoming line data and execute startup lines spindle_init(); coolant_init(); limits_init(); + plan_reset(); // Clear block buffer and planner variables st_reset(); // Clear stepper subsystem variables. // Sync cleared gcode and planner positions to current system position, which is only @@ -79,24 +88,13 @@ int main(void) sys.abort = false; sys.execute = 0; if (bit_istrue(settings.flags,BITFLAG_AUTO_START)) { sys.auto_start = true; } - - // Check for power-up and set system alarm if homing is enabled to force homing cycle - // by setting Grbl's alarm state. Alarm locks out all g-code commands, including the - // startup scripts, but allows access to settings and internal commands. Only a homing - // cycle '$H' or kill alarm locks '$X' will disable the alarm. - // NOTE: The startup script will run after successful completion of the homing cycle, but - // not after disabling the alarm locks. Prevents motion startup blocks from crashing into - // things uncontrollably. Very bad. - #ifdef HOMING_INIT_LOCK - if (sys.state == STATE_INIT && bit_istrue(settings.flags,BITFLAG_HOMING_ENABLE)) { sys.state = STATE_ALARM; } - #endif - + // Check for and report alarm state after a reset, error, or an initial power up. if (sys.state == STATE_ALARM) { report_feedback_message(MESSAGE_ALARM_LOCK); } else { // All systems go. Set system to ready and execute startup script. - sys.state = STATE_IDLE; + sys.state = STATE_IDLE; // Clear all state flags. protocol_execute_startup(); } } diff --git a/motion_control.c b/motion_control.c index e420c98..c9c46b6 100644 --- a/motion_control.c +++ b/motion_control.c @@ -213,12 +213,38 @@ void mc_dwell(float seconds) // Perform homing cycle to locate and set machine zero. Only '$H' executes this command. // NOTE: There should be no motions in the buffer and Grbl must be in an idle state before // executing the homing cycle. This prevents incorrect buffered plans after homing. -void mc_go_home() +void mc_homing_cycle() { sys.state = STATE_HOMING; // Set system state variable - LIMIT_PCMSK &= ~LIMIT_MASK; // Disable hard limits pin change register for cycle duration + limits_disable(); // Disable hard limits pin change register for cycle duration + plan_reset(); // Reset planner buffer before beginning homing routine. + st_reset(); // Reset step segment buffer before beginning homing routine. + + // ------------------------------------------------------------------------------------- + // Perform homing routine. NOTE: Special motion case. Only system reset works. - limits_go_home(); // Perform homing routine. + // Search to engage all axes limit switches at faster homing seek rate. + limits_go_home(HOMING_SEARCH_CYCLE_0, true, false, settings.homing_seek_rate); // Search cycle 0 + #ifdef HOMING_SEARCH_CYCLE_1 + limits_go_home(HOMING_SEARCH_CYCLE_1, true, false, settings.homing_seek_rate); // Search cycle 1 + #endif + #ifdef HOMING_SEARCH_CYCLE_2 + limits_go_home(HOMING_SEARCH_CYCLE_2, true, false, settings.homing_seek_rate); // Search cycle 2 + #endif + + // Now in proximity of all limits. Carefully leave and approach switches in multiple cycles + // to precisely hone in on the machine zero location. Moves at slower homing feed rate. + int8_t n_cycle = N_HOMING_LOCATE_CYCLE; + while (n_cycle--) { + // Leave all switches to release them. After cycles complete, this is machine zero. + limits_go_home(HOMING_LOCATE_CYCLE, false, true, settings.homing_feed_rate); + + if (n_cycle > 0) { + // Re-approach all switches to re-engage them. + limits_go_home(HOMING_LOCATE_CYCLE, true, false, settings.homing_feed_rate); + } + } + // ------------------------------------------------------------------------------------- protocol_execute_runtime(); // Check for reset and set system abort. if (sys.abort) { return; } // Did not complete. Alarm state set by mc_alarm. @@ -254,12 +280,13 @@ void mc_go_home() mc_line(pulloff_target, settings.homing_seek_rate, false); st_cycle_start(); // Move it. Nothing should be in the buffer except this motion. plan_synchronize(); // Make sure the motion completes. + // NOTE: Stepper idle lock resumes normal functionality after cycle. // The gcode parser position circumvented by the pull-off maneuver, so sync position now. gc_sync_position(); // If hard limits feature enabled, re-enable hard limits pin change register after homing cycle. - if (bit_istrue(settings.flags,BITFLAG_HARD_LIMIT_ENABLE)) { LIMIT_PCMSK |= LIMIT_MASK; } + limits_init(); // Finished! } @@ -294,10 +321,9 @@ void mc_reset() // NOTE: If steppers are kept enabled via the step idle delay setting, this also keeps // the steppers enabled by avoiding the go_idle call altogether, unless the motion state is // violated, by which, all bets are off. - switch (sys.state) { - case STATE_CYCLE: case STATE_HOLD: case STATE_HOMING: // case STATE_JOG: - sys.execute |= EXEC_ALARM; // Execute alarm state. - st_go_idle(); // Execute alarm force kills steppers. Position likely lost. + if (sys.state & (STATE_CYCLE | STATE_HOLD | STATE_HOMING)) { + sys.execute |= EXEC_ALARM; // Execute alarm state. + st_go_idle(); // Execute alarm force kills steppers. Position likely lost. } } } diff --git a/motion_control.h b/motion_control.h index 50282a0..3ca8d30 100644 --- a/motion_control.h +++ b/motion_control.h @@ -41,7 +41,7 @@ void mc_arc(float *position, float *target, float *offset, uint8_t axis_0, uint8 void mc_dwell(float seconds); // Perform homing cycle to locate machine zero. Requires limit switches. -void mc_go_home(); +void mc_homing_cycle(); // Performs system reset. If in motion state, kills all motion and sets system alarm. void mc_reset(); diff --git a/nuts_bolts.h b/nuts_bolts.h index 59aeff6..554ee31 100644 --- a/nuts_bolts.h +++ b/nuts_bolts.h @@ -74,15 +74,14 @@ // Define system state bit map. The state variable primarily tracks the individual functions // of Grbl to manage each without overlapping. It is also used as a messaging flag for // critical events. -#define STATE_IDLE 0 // Must be zero. -#define STATE_INIT 1 // Initial power up state. -#define STATE_QUEUED 2 // Indicates buffered blocks, awaiting cycle start. -#define STATE_CYCLE 3 // Cycle is running -#define STATE_HOLD 4 // Executing feed hold -#define STATE_HOMING 5 // Performing homing cycle -#define STATE_ALARM 6 // In alarm state. Locks out all g-code processes. Allows settings access. -#define STATE_CHECK_MODE 7 // G-code check mode. Locks out planner and motion only. -// #define STATE_JOG 8 // Jogging mode is unique like homing. +#define STATE_IDLE 0 // Must be zero. No flags. +#define STATE_QUEUED bit(0) // Indicates buffered blocks, awaiting cycle start. +#define STATE_CYCLE bit(1) // Cycle is running +#define STATE_HOLD bit(2) // Executing feed hold +#define STATE_HOMING bit(3) // Performing homing cycle +#define STATE_ALARM bit(4) // In alarm state. Locks out all g-code processes. Allows settings access. +#define STATE_CHECK_MODE bit(5) // G-code check mode. Locks out planner and motion only. +// #define STATE_JOG bit(6) // Jogging mode is unique like homing. // Define global system variables typedef struct { diff --git a/planner.c b/planner.c index d4b4326..9a91406 100644 --- a/planner.c +++ b/planner.c @@ -207,7 +207,7 @@ static void planner_recalculate() } -void plan_init() +void plan_reset() { memset(&pl, 0, sizeof(pl)); // Clear planner struct block_buffer_tail = 0; @@ -255,7 +255,7 @@ uint8_t plan_check_full_buffer() // during a synchronize call, if it should happen. Also, waits for clean cycle end. void plan_synchronize() { - while (plan_get_current_block() || sys.state == STATE_CYCLE) { + while (plan_get_current_block() || (sys.state == STATE_CYCLE)) { protocol_execute_runtime(); // Check and execute run-time commands if (sys.abort) { return; } // Check for system abort } diff --git a/planner.h b/planner.h index dc96830..3ec3bea 100644 --- a/planner.h +++ b/planner.h @@ -34,8 +34,8 @@ typedef struct { // Fields used by the bresenham algorithm for tracing the line // NOTE: Used by stepper algorithm to execute the block correctly. Do not alter these values. uint8_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) - int32_t steps[N_AXIS]; // Step count along each axis - int32_t step_event_count; // The maximum step axis count and number of steps required to complete this block. + uint32_t steps[N_AXIS]; // Step count along each axis + uint32_t step_event_count; // The maximum step axis count and number of steps required to complete this block. // Fields used by the motion planner to manage acceleration float entry_speed_sqr; // The current planned entry speed at block junction in (mm/min)^2 @@ -49,8 +49,8 @@ typedef struct { } plan_block_t; -// Initialize the motion plan subsystem -void plan_init(); +// Initialize and reset the motion plan subsystem +void plan_reset(); // Add a new linear movement to the buffer. target[N_AXIS] is the signed, absolute target position // in millimeters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed diff --git a/protocol.c b/protocol.c index 014b607..103d2f4 100644 --- a/protocol.c +++ b/protocol.c @@ -235,8 +235,8 @@ uint8_t protocol_execute_line(char *line) case 'H' : // Perform homing cycle if (bit_istrue(settings.flags,BITFLAG_HOMING_ENABLE)) { // Only perform homing if Grbl is idle or lost. - if ( sys.state==STATE_IDLE || sys.state==STATE_ALARM ) { - mc_go_home(); + if ( sys.state == STATE_IDLE || sys.state == STATE_ALARM ) { + mc_homing_cycle(); if (!sys.abort) { protocol_execute_startup(); } // Execute startup scripts after successful homing. } else { return(STATUS_IDLE_ERROR); } } else { return(STATUS_SETTING_DISABLED); } diff --git a/report.c b/report.c index cf3d776..c44efe0 100644 --- a/report.c +++ b/report.c @@ -308,7 +308,6 @@ void report_realtime_status() // Report current machine state switch (sys.state) { case STATE_IDLE: printPgmString(PSTR(" 0 + uint8_t step_bits; // Stores out_bits output to complete the step pulse delay + #endif // Used by the stepper driver interrupt uint8_t execute_step; // Flags step execution for each interrupt. uint8_t step_pulse_time; // Step pulse reset time after step rise uint8_t out_bits; // The next stepping-bits to be output - uint8_t step_count; // Steps remaining in line segment motion + uint16_t step_count; // Steps remaining in line segment motion uint8_t exec_block_index; // Tracks the current st_block index. Change indicates new block. st_block_t *exec_block; // Pointer to the block data for the segment being executed segment_t *exec_segment; // Pointer to the segment being executed @@ -106,6 +105,10 @@ typedef struct { float step_per_mm; // Current planner block step/millimeter conversion scalar float steps_remaining; + float dt_remainder; + + float mm_per_step; + float minimum_mm; uint8_t ramp_type; // Current segment ramp state float mm_complete; // End of velocity profile from end of current planner block in (mm). @@ -166,16 +169,23 @@ void st_wake_up() } else { STEPPERS_DISABLE_PORT &= ~(1<> 3); - + + // Initialize step pulse timing from settings. Here to ensure updating after re-writing. + #ifdef STEP_PULSE_DELAY + // Set total step pulse time after direction pin set. Ad hoc computation from oscilloscope. + st.step_pulse_time = -(((settings.pulse_microseconds+STEP_PULSE_DELAY-2)*TICKS_PER_MICROSECOND) >> 3); + // Set delay between direction pin write and step command. + OCR2A = -(((settings.pulse_microseconds)*TICKS_PER_MICROSECOND) >> 3); + #else // Normal operation + // Set step pulse time. Ad hoc computation from oscilloscope. Uses two's complement. + st.step_pulse_time = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3); + #endif + // Enable stepper driver interrupt - TCNT2 = 0; // Clear Timer2 - TIMSK2 |= (1<n_step; - + // Initialize step segment timing per step and load number of steps to execute. + TCCR1B = (TCCR1B & ~(0x07<prescaler<cycles_per_tick; + st.step_count = st.exec_segment->n_step; // NOTE: Can sometimes be zero when moving slow. // If the new segment starts a new planner block, initialize stepper variables and counters. // NOTE: When the segment data index changes, this indicates a new planner block. if ( st.exec_block_index != st.exec_segment->st_block_index ) { st.exec_block_index = st.exec_segment->st_block_index; st.exec_block = &st_block_buffer[st.exec_block_index]; - - // Initialize direction bits for block. Set execute flag to set directions bits upon next ISR tick. - st.out_bits = st.exec_block->direction_bits ^ settings.invert_mask; - st.execute_step = true; - // Initialize Bresenham line and distance counters st.counter_x = (st.exec_block->step_event_count >> 1); st.counter_y = st.counter_x; st.counter_z = st.counter_x; - st.counter_dist = 0; } - } else { // Segment buffer empty. Shutdown. st_go_idle(); bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end return; // Nothing to do but exit. } - - } - - // Iterate inverse time counter. Triggers each Bresenham step event. - st.counter_dist += st.exec_segment->dist_per_tick; - - // Execute Bresenham step event, when it's time to do so. - if (st.counter_dist > STEP_FTOL_MULTIPLIER) { - if (st.step_count > 0) { // Block phase correction from executing step. - st.counter_dist -= STEP_FTOL_MULTIPLIER; // Reload inverse time counter - st.step_count--; // Decrement step events count + } + + // Reset out_bits and reload direction bits + st.out_bits = st.exec_block->direction_bits; - // Execute step displacement profile by Bresenham line algorithm - st.execute_step = true; - st.out_bits = st.exec_block->direction_bits; // Reset out_bits and reload direction bits - st.counter_x -= st.exec_block->steps[X_AXIS]; - if (st.counter_x < 0) { - st.out_bits |= (1<step_event_count; - if (st.out_bits & (1<steps[Y_AXIS]; - if (st.counter_y < 0) { - st.out_bits |= (1<step_event_count; - if (st.out_bits & (1<steps[Z_AXIS]; - if (st.counter_z < 0) { - st.out_bits |= (1<step_event_count; - if (st.out_bits & (1< 0) { + st.step_count--; // Decrement step events count + + st.counter_x += st.exec_block->steps[X_AXIS]; + if (st.counter_x > st.exec_block->step_event_count) { + st.out_bits |= (1<step_event_count; + if (st.out_bits & (1<steps[Y_AXIS]; + if (st.counter_y > st.exec_block->step_event_count) { + st.out_bits |= (1<step_event_count; + if (st.out_bits & (1<steps[Z_AXIS]; + if (st.counter_z > st.exec_block->step_event_count) { + st.out_bits |= (1<step_event_count; + if (st.out_bits & (1< st.exec_segment->phase_dist) { - // Segment is complete. Discard current segment and advance segment indexing. - st.exec_segment = NULL; - if ( ++segment_buffer_tail == SEGMENT_BUFFER_SIZE) { segment_buffer_tail = 0; } - } + // Segment is complete. Discard current segment and advance segment indexing. + st.exec_segment = NULL; + if ( ++segment_buffer_tail == SEGMENT_BUFFER_SIZE) { segment_buffer_tail = 0; } } - + + st.out_bits ^= settings.invert_mask; // Apply step port invert mask busy = false; // SPINDLE_ENABLE_PORT ^= 1<direction_bits = pl_block->direction_bits; st_prep_block->steps[X_AXIS] = pl_block->steps[X_AXIS]; st_prep_block->steps[Y_AXIS] = pl_block->steps[Y_AXIS]; st_prep_block->steps[Z_AXIS] = pl_block->steps[Z_AXIS]; - st_prep_block->direction_bits = pl_block->direction_bits; st_prep_block->step_event_count = pl_block->step_event_count; // Initialize segment buffer data for generating the segments. - prep.steps_remaining = st_prep_block->step_event_count; + prep.steps_remaining = pl_block->step_event_count; prep.step_per_mm = prep.steps_remaining/pl_block->millimeters; + prep.mm_per_step = pl_block->millimeters/prep.steps_remaining; + prep.minimum_mm = pl_block->millimeters-prep.mm_per_step; if (sys.state == STATE_HOLD) { // Override planner block entry speed and enforce deceleration during feed hold. @@ -495,6 +528,8 @@ void st_prep_buffer() pl_block->entry_speed_sqr = prep.exit_speed*prep.exit_speed; } else { prep.current_speed = sqrt(pl_block->entry_speed_sqr); } + + prep.dt_remainder = 0.0; } /* --------------------------------------------------------------------------------- @@ -574,18 +609,19 @@ void st_prep_buffer() may range from zero to the length of the block. Velocity profiles can end either at the end of planner block (typical) or mid-block at the end of a forced deceleration, such as from a feed hold. - */ + */ + float dt_max = DT_SEGMENT; float dt = 0.0; float mm_remaining = pl_block->millimeters; - float time_var = DT_SEGMENT; // Time worker variable + float time_var = dt_max; // Time worker variable float mm_var; // mm-Distance worker variable - float speed_var; // Speed work variable. + float speed_var; // Speed worker variable. do { switch (prep.ramp_type) { case RAMP_ACCEL: // NOTE: Acceleration ramp only computes during first do-while loop. - speed_var = pl_block->acceleration*DT_SEGMENT; - mm_remaining -= DT_SEGMENT*(prep.current_speed + 0.5*speed_var); + speed_var = pl_block->acceleration*dt_max; + mm_remaining -= dt_max*(prep.current_speed + 0.5*speed_var); if (mm_remaining < prep.accelerate_until) { // End of acceleration ramp. // Acceleration-cruise, acceleration-deceleration ramp junction, or end of block. mm_remaining = prep.accelerate_until; // NOTE: 0.0 at EOB @@ -618,15 +654,18 @@ void st_prep_buffer() if (mm_var > prep.mm_complete) { // Deceleration only. mm_remaining = mm_var; prep.current_speed -= speed_var; - break; // Segment complete. Exit switch-case statement. + break; // Segment complete. Exit switch-case statement. Continue do-while loop. } } // End of block or end of forced-deceleration. time_var = 2.0*(mm_remaining-prep.mm_complete)/(prep.current_speed+prep.exit_speed); mm_remaining = prep.mm_complete; } dt += time_var; // Add computed ramp time to total segment time. - if (dt < DT_SEGMENT) { time_var = DT_SEGMENT - dt; } // **Incomplete** At ramp junction. - else { break; } // **Complete** Exit loop. Segment execution time maxed. + if (dt < dt_max) { time_var = dt_max - dt; } // **Incomplete** At ramp junction. + else if (mm_remaining > prep.minimum_mm) { // Check for slow segments with zero steps. + dt_max += DT_SEGMENT; // Increase segment time to ensure at least one step in segment. + time_var = dt_max - dt; + } else { break; } // **Complete** Exit loop. Segment execution time maxed. } while (mm_remaining > prep.mm_complete); // **Complete** Exit loop. Profile complete. /* ----------------------------------------------------------------------------------- @@ -639,48 +678,69 @@ void st_prep_buffer() Fortunately, this scenario is highly unlikely and unrealistic in CNC machines supported by Grbl (i.e. exceeding 10 meters axis travel at 200 step/mm). */ - // Use time_var to pre-compute dt inversion with integer multiplier. - time_var = (STEP_FTOL_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))/dt; // (ftol_mult/isr_tic) - if (mm_remaining > 0.0) { // Block still incomplete. Distance remaining to be executed. - float steps_remaining = prep.step_per_mm*mm_remaining; - prep_segment->dist_per_tick = ceil( (prep.steps_remaining-steps_remaining)*time_var ); // (ftol_mult*step/isr_tic) + uint32_t cycles; + float steps_remaining = prep.step_per_mm*mm_remaining; - // Compute number of steps to execute and segment step phase correction. - prep_segment->phase_dist = ceil(STEP_FTOL_MULTIPLIER*(ceil(steps_remaining)-steps_remaining)); - prep_segment->n_step = ceil(prep.steps_remaining)-ceil(steps_remaining); + float inv_rate = dt/(prep.steps_remaining-steps_remaining); + cycles = ceil( (TICKS_PER_MICROSECOND*1000000*60)*inv_rate ); // (ftol_mult*step/isr_tic) - // Update step execution variables. - if (mm_remaining == prep.mm_complete) { + // Compute number of steps to execute and segment step phase correction. + prep_segment->n_step = ceil(prep.steps_remaining)-ceil(steps_remaining); + + // Determine end of segment conditions. Setup initial conditions for next segment. + if (mm_remaining > prep.mm_complete) { + // Normal operation. Block incomplete. Distance remaining to be executed. + prep.minimum_mm = prep.mm_per_step*floor(steps_remaining); + pl_block->millimeters = mm_remaining; + prep.steps_remaining = steps_remaining; + } else { + // End of planner block or forced-termination. No more distance to be executed. + if (mm_remaining > 0.0) { // At end of forced-termination. // NOTE: Currently only feed holds qualify for this scenario. May change with overrides. prep.current_speed = 0.0; prep.steps_remaining = ceil(steps_remaining); - pl_block->millimeters = prep.steps_remaining/prep.step_per_mm; // Update with full steps. + prep.minimum_mm = prep.steps_remaining-prep.mm_per_step; + pl_block->millimeters = prep.steps_remaining*prep.mm_per_step; // Update with full steps. plan_cycle_reinitialize(); sys.state = STATE_QUEUED; // End cycle. - } else { - pl_block->millimeters = mm_remaining; - prep.steps_remaining = steps_remaining; - } + } else { // End of planner block + // The planner block is complete. All steps are set to be executed in the segment buffer. + pl_block = NULL; + plan_discard_current_block(); - } else { // End of block. - // Set to execute the remaining steps and no phase correction upon finishing the block. - prep_segment->dist_per_tick = ceil( prep.steps_remaining*time_var ); // (mult*step/isr_tic) - prep_segment->phase_dist = 0; - prep_segment->n_step = ceil(prep.steps_remaining); - - // The planner block is complete. All steps are set to be executed in the segment buffer. - // TODO: Broken with feed holds. Need to recalculate the planner buffer at this time. - pl_block = NULL; - plan_discard_current_block(); - - if (sys.state == STATE_HOLD) { - if (prep.current_speed == 0.0) { - plan_cycle_reinitialize(); - sys.state = STATE_QUEUED; + if (sys.state == STATE_HOLD) { + if (prep.current_speed == 0.0) { + // TODO: Check if the segment buffer gets initialized correctly. + plan_cycle_reinitialize(); + sys.state = STATE_QUEUED; + } } } } - + +// TODO: Compile-time checks to what prescalers need to be compiled in. + if (cycles < (1UL << 16)) { // < 65536 (4.1ms @ 16MHz) + prep_segment->prescaler = 1; // prescaler: 0 + prep_segment->cycles_per_tick = cycles; + } else { + prep_segment->prescaler = 2; // prescaler: 8 + if (cycles < (1UL << 19)) { // < 524288 (32.8ms@16MHz) + prep_segment->cycles_per_tick = cycles >> 3; +// } else if (cycles < (1UL << 22)) { // < 4194304 (262ms@16MHz) +// prep_segment->prescaler = 3; // prescaler: 64 +// prep_segment->cycles_per_tick = cycles >> 6; +// } else if (cycles < (1UL << 24)) { // < 16777216 (1.05sec@16MHz) +// prep_segment->prescaler = 4; // prescaler: 256 +// prep_segment->cycles_per_tick = (cycles >> 8); +// } else { +// prep_segment->prescaler = 5; // prescaler: 1024 +// if (cycles < (1UL << 26)) { // < 67108864 (4.19sec@16MHz) +// prep_segment->cycles_per_tick = (cycles >> 10); + } else { // Just set the slowest speed possible. + prep_segment->cycles_per_tick = 0xffff; + } + } + // New step segment initialization completed. Increment segment buffer indices. segment_buffer_head = segment_next_head; if ( ++segment_next_head == SEGMENT_BUFFER_SIZE ) { segment_next_head = 0; } @@ -689,7 +749,7 @@ void st_prep_buffer() // if (blength < 0) { blength += SEGMENT_BUFFER_SIZE; } // printInteger(blength); - if ((sys.state == STATE_HOMING) || (sys.state == STATE_QUEUED)) { return; } // Force only one prepped segment. + if (sys.state & (STATE_QUEUED | STATE_HOMING)) { return; } // Force exit or one prepped segment. } } diff --git a/stepper.h b/stepper.h index e2187b2..9fc2c73 100644 --- a/stepper.h +++ b/stepper.h @@ -22,10 +22,8 @@ #ifndef stepper_h #define stepper_h -#include - #ifndef SEGMENT_BUFFER_SIZE - #define SEGMENT_BUFFER_SIZE 7 + #define SEGMENT_BUFFER_SIZE 6 #endif // Initialize and setup the stepper motor subsystem From 5ab2bb7767730ee32960fd53793721ec27f91fd9 Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Sun, 29 Dec 2013 20:51:48 -0700 Subject: [PATCH 29/73] Incomplete dev code push, but working. Lots of updates/fixes/improvements. Much still to polish. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Ugh. Github just erased my list of improvements and changes due to a conflict and forcing me to resolve it. Hope this goes through. - Major stepper algorithm change. Trashed the old v0.9 edge branch-style stepper algorithm. It’s fine, but it was susceptible to aliasing noise when moving very slow or very fast. It also had a bit of CPU overhead. It was written to solve a standing issue with v0.8 master, where it couldn’t generate a smooth acceleration abocve 10-15kHz. But, with new step segment buffer in v0.9c, it inadvertently fixed the acceleration problem with v0.8 stepper algorithm. So, what does it mean for you? Smoother stepper pulses and likely higher step frequencies. - Stepper algorithm now uses Timer1 and Timer2, instead of Timer0 and Timer2. Timers 0 and 2 can be swapped if there is an issue. - With the old v0.8 stepper algorithm, the STEP_DELAY_PULSE configuration option is also back. - NEW! Hard limit software debouncing. Grbl now employs the AVR’s watchdog timer as a way to monitor the hard limit pins and checking their states after a delay. This is a simple software debouncing technique and may help alleviate some of the false trigger some users have been complaining about. BUT, this won’t fix electric noise issues! - Fixed an issue with the new homing cycle routine where it wasn’t honoring the acceleration and axis speed limits depending on the homing cycle mask. Now does. Also, updated the homing direction mask code to be a little cleaner. - Moved the main part of the homing cycle control and execution to motion_control.c, where it fits better. - Removed the STATE_INIT system state as it was redundant. Made the system states into bitflags so multiple system states can be checked via one if statement. - Reorganized the power-up routine to work with the new system states. --- stepper.c | 3 --- 1 file changed, 3 deletions(-) diff --git a/stepper.c b/stepper.c index 63ee2dc..b02c9d5 100644 --- a/stepper.c +++ b/stepper.c @@ -105,7 +105,6 @@ typedef struct { float step_per_mm; // Current planner block step/millimeter conversion scalar float steps_remaining; - float dt_remainder; float mm_per_step; float minimum_mm; @@ -528,8 +527,6 @@ void st_prep_buffer() pl_block->entry_speed_sqr = prep.exit_speed*prep.exit_speed; } else { prep.current_speed = sqrt(pl_block->entry_speed_sqr); } - - prep.dt_remainder = 0.0; } /* --------------------------------------------------------------------------------- From 47cd40c8dce6fac545119508459a4f2f0610f9ee Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Mon, 30 Dec 2013 18:44:46 -0700 Subject: [PATCH 30/73] Incomplete push but working. Lots more stuff. More to come. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - NEW! An active multi-axis step smoothing algorithm that automatically adjusts dependent on step frequency. This solves the long standing issue to aliasing when moving with multiple axes. Similar in scheme to Smoothieware, but more advanced in ensuring a more consistent CPU overhead throughout all frequencies while maintaining step exactness. - Switched from Timer2 to Timer0 for the Step Port Reset Interrupt. Mainly to free up hardware PWM pins. - Seperated the direction and step pin assignments, so we can now move them to seperate ports. This means that we can more easily support 4+ axes in the future. - Added a setting for inverting the limit pins, as so many users have request. Better late than never. - Bug fix related to EEPROM calls when in cycle. The EEPROM would kill the stepper motion. Now protocol mandates that the system be either in IDLE or ALARM to access or change any settings. - Bug fix related to resuming the cycle after a spindle or dwell command if auto start has been disabled. This fix is somewhat temporary or more of a patch. Doesn’t work with a straight call-response streaming protocol, but works fine with serial buffer pre-filling streaming that most clients use. - Renamed the pin_map.h to cpu_map.h to more accurately describe what the file is. - Pushed an auto start bug fix upon re-initialization. - Much more polishing to do! --- config.h | 37 +++---- pin_map.h => cpu_map.h | 39 ++++--- defaults.h | 17 ++- limits.c | 22 ++-- limits.h | 2 +- main.c | 1 + motion_control.c | 30 ++++-- nuts_bolts.h | 2 +- planner.c | 1 + protocol.c | 144 +++++++++++++------------ report.c | 39 +++---- settings.c | 43 ++++---- settings.h | 6 +- stepper.c | 237 ++++++++++++++++++++++++----------------- 14 files changed, 348 insertions(+), 272 deletions(-) rename pin_map.h => cpu_map.h (86%) diff --git a/config.h b/config.h index aad11c3..e46aa81 100644 --- a/config.h +++ b/config.h @@ -34,9 +34,9 @@ // Serial baud rate #define BAUD_RATE 115200 -// Default pin mappings. Grbl officially supports the Arduino Uno only. Other processor types -// may exist from user-supplied templates or directly user-defined in pin_map.h -#define PIN_MAP_ARDUINO_UNO +// Default cpu mappings. Grbl officially supports the Arduino Uno only. Other processor types +// may exist from user-supplied templates or directly user-defined in cpu_map.h +#define CPU_MAP_ATMEGA328P // Arduino Uno CPU // Define runtime command special characters. These characters are 'picked-off' directly from the // serial read data stream and are not passed to the grbl line execution parser. Select characters @@ -49,12 +49,6 @@ #define CMD_CYCLE_START '~' #define CMD_RESET 0x18 // ctrl-x. -// Uncomment the following define if you are using hardware that drives high when your limits -// are reached. You will need to ensure that you have appropriate pull-down resistors on the -// limit switch input pins, or that your hardware drives the pins low when they are open (non- -// triggered). -// #define LIMIT_SWITCHES_ACTIVE_HIGH // Uncomment to enable - // If homing is enabled, homing init lock sets Grbl into an alarm state upon power up. This forces // the user to perform the homing cycle (or override the locks) before doing anything else. This is // mainly a safety feature to remind the user to home, since position is unknown to Grbl. @@ -94,16 +88,8 @@ // set this only as high as needed. Approximate successful values can widely range from 50 to 200 or more. #define ACCELERATION_TICKS_PER_SECOND 100 -// Creates a delay between the direction pin setting and corresponding step pulse by creating -// another interrupt (Timer2 compare) to manage it. The main Grbl interrupt (Timer1 compare) -// sets the direction pins, and does not immediately set the stepper pins, as it would in -// normal operation. The Timer2 compare fires next to set the stepper pins after the step -// pulse delay time, and Timer2 overflow will complete the step pulse, except now delayed -// by the step pulse time plus the step pulse delay. (Thanks langwadt for the idea!) -// NOTE: Uncomment to enable. The recommended delay must be > 3us, and, when added with the -// user-supplied step pulse time, the total time must not exceed 127us. Reported successful -// values for certain setups have ranged from 5 to 20us. -// #define STEP_PULSE_DELAY 10 // Step pulse delay in microseconds. Default disabled. +#define ACTIVE_MULTI_AXIS_STEP_SMOOTHING +#define ENABLE_SOFTWARE_DEBOUNCE // Minimum planner junction speed. Sets the default minimum junction speed the planner plans to at // every buffer block junction, except for starting from rest and end of the buffer, which are always @@ -127,6 +113,17 @@ // time step. Also, keep in mind that the Arduino delay timer is not very accurate for long delays. #define DWELL_TIME_STEP 50 // Integer (1-255) (milliseconds) +// Creates a delay between the direction pin setting and corresponding step pulse by creating +// another interrupt (Timer2 compare) to manage it. The main Grbl interrupt (Timer1 compare) +// sets the direction pins, and does not immediately set the stepper pins, as it would in +// normal operation. The Timer2 compare fires next to set the stepper pins after the step +// pulse delay time, and Timer2 overflow will complete the step pulse, except now delayed +// by the step pulse time plus the step pulse delay. (Thanks langwadt for the idea!) +// NOTE: Uncomment to enable. The recommended delay must be > 3us, and, when added with the +// user-supplied step pulse time, the total time must not exceed 127us. Reported successful +// values for certain setups have ranged from 5 to 20us. +// #define STEP_PULSE_DELAY 10 // Step pulse delay in microseconds. Default disabled. + // The number of linear motions in the planner buffer to be planned at any give time. The vast // majority of RAM that Grbl uses is based on this buffer size. Only increase if there is extra // available RAM, like when re-compiling for a Mega or Sanguino. Or decrease if the Arduino @@ -171,8 +168,6 @@ // case, please report any successes to grbl administrators! // #define ENABLE_XONXOFF // Default disabled. Uncomment to enable. -#define ENABLE_SOFTWARE_DEBOUNCE - // --------------------------------------------------------------------------------------- // TODO: Install compile-time option to send numeric status codes rather than strings. diff --git a/pin_map.h b/cpu_map.h similarity index 86% rename from pin_map.h rename to cpu_map.h index 233b32a..0d21fe3 100644 --- a/pin_map.h +++ b/cpu_map.h @@ -1,5 +1,5 @@ /* - pin_map.h - Pin mapping configuration file + cpu_map.h - CPU and pin mapping configuration file Part of Grbl Copyright (c) 2013 Sungeun K. Jeon @@ -18,14 +18,17 @@ along with Grbl. If not, see . */ -/* The pin_map.h file serves as a central pin mapping settings file for different processor +/* The cpu_map.h file serves as a central pin mapping settings file for different processor types, i.e. AVR 328p or AVR Mega 2560. Grbl officially supports the Arduino Uno, but the other supplied pin mappings are supplied by users, so your results may vary. */ -#ifndef pin_map_h -#define pin_map_h +// NOTE: This is still a work in progress. We are still centralizing the configurations to +// this file, so your success may vary for other CPUs. -#ifdef PIN_MAP_ARDUINO_UNO // AVR 328p, Officially supported by Grbl. +#ifndef cpu_map_h +#define cpu_map_h + +#ifdef CPU_MAP_ATMEGA328P // (Arduino Uno) Officially supported by Grbl. // Serial port pins #define SERIAL_RX USART_RX_vect @@ -37,12 +40,14 @@ #define X_STEP_BIT 2 // Uno Digital Pin 2 #define Y_STEP_BIT 3 // Uno Digital Pin 3 #define Z_STEP_BIT 4 // Uno Digital Pin 4 + #define STEP_MASK ((1< 0) { // Re-approach all switches to re-engage them. - limits_go_home(HOMING_LOCATE_CYCLE, true, false, settings.homing_feed_rate); + limits_go_home(HOMING_LOCATE_CYCLE, true, settings.homing_feed_rate); } } // ------------------------------------------------------------------------------------- @@ -291,15 +291,23 @@ void mc_homing_cycle() } -// Auto-cycle start is a user setting that automatically begins the cycle when a user enters -// a valid motion command either manually or by a streaming tool. This is intended as a beginners -// feature to help new users to understand g-code. It can be disabled. Otherwise, the normal -// operation of cycle start is manually issuing a cycle start command whenever the user is -// ready and there is a valid motion command in the planner queue. +// Auto-cycle start has two purposes: 1. Resumes a plan_synchronize() call from a function that +// requires the planner buffer to empty (spindle enable, dwell, etc.) 2. As a user setting that +// automatically begins the cycle when a user enters a valid motion command manually. This is +// intended as a beginners feature to help new users to understand g-code. It can be disabled +// as a beginner tool, but (1.) still operates. If disabled, the operation of cycle start is +// manually issuing a cycle start command whenever the user is ready and there is a valid motion +// command in the planner queue. // NOTE: This function is called from the main loop and mc_line() only and executes when one of // two conditions exist respectively: There are no more blocks sent (i.e. streaming is finished, // single commands), or the planner buffer is full and ready to go. -void mc_auto_cycle_start() { if (sys.auto_start) { st_cycle_start(); } } +void mc_auto_cycle_start() +{ + if (sys.auto_start) { + st_cycle_start(); + if (bit_isfalse(settings.flags,BITFLAG_AUTO_START)) { sys.auto_start = false; } // Reset auto start per settings. + } +} // Method to ready the system to reset by setting the runtime reset command and killing any diff --git a/nuts_bolts.h b/nuts_bolts.h index 554ee31..ab19485 100644 --- a/nuts_bolts.h +++ b/nuts_bolts.h @@ -27,7 +27,7 @@ #include #include "config.h" #include "defaults.h" -#include "pin_map.h" +#include "cpu_map.h" #define false 0 #define true 1 diff --git a/planner.c b/planner.c index 9a91406..09a2c44 100644 --- a/planner.c +++ b/planner.c @@ -255,6 +255,7 @@ uint8_t plan_check_full_buffer() // during a synchronize call, if it should happen. Also, waits for clean cycle end. void plan_synchronize() { + sys.auto_start = true; // Set auto start to resume cycle after synchronize and caller completes. while (plan_get_current_block() || (sys.state == STATE_CYCLE)) { protocol_execute_runtime(); // Check and execute run-time commands if (sys.abort) { return; } // Check for system abort diff --git a/protocol.c b/protocol.c index 103d2f4..1e8e9cb 100644 --- a/protocol.c +++ b/protocol.c @@ -192,16 +192,10 @@ uint8_t protocol_execute_line(char *line) { // Grbl internal command and parameter lines are of the form '$4=374.3' or '$' for help if(line[0] == '$') { - uint8_t char_counter = 1; uint8_t helper_var = 0; // Helper variable float parameter, value; switch( line[char_counter] ) { - case 0 : report_grbl_help(); break; - case '$' : // Prints Grbl settings - if ( line[++char_counter] != 0 ) { return(STATUS_UNSUPPORTED_STATEMENT); } - else { report_grbl_settings(); } - break; case '#' : // Print gcode parameters if ( line[++char_counter] != 0 ) { return(STATUS_UNSUPPORTED_STATEMENT); } else { report_gcode_parameters(); } @@ -210,37 +204,6 @@ uint8_t protocol_execute_line(char *line) if ( line[++char_counter] != 0 ) { return(STATUS_UNSUPPORTED_STATEMENT); } else { report_gcode_modes(); } break; - case 'C' : // Set check g-code mode - if ( line[++char_counter] != 0 ) { return(STATUS_UNSUPPORTED_STATEMENT); } - // Perform reset when toggling off. Check g-code mode should only work if Grbl - // is idle and ready, regardless of alarm locks. This is mainly to keep things - // simple and consistent. - if ( sys.state == STATE_CHECK_MODE ) { - mc_reset(); - report_feedback_message(MESSAGE_DISABLED); - } else { - if (sys.state) { return(STATUS_IDLE_ERROR); } - sys.state = STATE_CHECK_MODE; - report_feedback_message(MESSAGE_ENABLED); - } - break; - case 'X' : // Disable alarm lock - if ( line[++char_counter] != 0 ) { return(STATUS_UNSUPPORTED_STATEMENT); } - if (sys.state == STATE_ALARM) { - report_feedback_message(MESSAGE_ALARM_UNLOCK); - sys.state = STATE_IDLE; - // Don't run startup script. Prevents stored moves in startup from causing accidents. - } - break; - case 'H' : // Perform homing cycle - if (bit_istrue(settings.flags,BITFLAG_HOMING_ENABLE)) { - // Only perform homing if Grbl is idle or lost. - if ( sys.state == STATE_IDLE || sys.state == STATE_ALARM ) { - mc_homing_cycle(); - if (!sys.abort) { protocol_execute_startup(); } // Execute startup scripts after successful homing. - } else { return(STATUS_IDLE_ERROR); } - } else { return(STATUS_SETTING_DISABLED); } - break; // case 'J' : break; // Jogging methods // TODO: Here jogging can be placed for execution as a seperate subprogram. It does not need to be // susceptible to other runtime commands except for e-stop. The jogging function is intended to @@ -252,42 +215,81 @@ uint8_t protocol_execute_line(char *line) // More controlled exact motions can be taken care of by inputting G0 or G1 commands, which are // handled by the planner. It would be possible for the jog subprogram to insert blocks into the // block buffer without having the planner plan them. It would need to manage de/ac-celerations - // on its own carefully. This approach could be effective and possibly size/memory efficient. - case 'N' : // Startup lines. - if ( line[++char_counter] == 0 ) { // Print startup lines - for (helper_var=0; helper_var < N_STARTUP_LINE; helper_var++) { - if (!(settings_read_startup_line(helper_var, line))) { - report_status_message(STATUS_SETTING_READ_FAIL); + // on its own carefully. This approach could be effective and possibly size/memory efficient. + default : + // Block any system command that requires the state as IDLE/ALARM. (i.e. EEPROM, homing) + if ( !(sys.state == STATE_IDLE || sys.state == STATE_ALARM) ) { return(STATUS_IDLE_ERROR); } + switch( line[char_counter] ) { + case 0 : report_grbl_help(); break; + case '$' : // Prints Grbl settings + if ( line[++char_counter] != 0 ) { return(STATUS_UNSUPPORTED_STATEMENT); } + else { report_grbl_settings(); } + break; + case 'C' : // Set check g-code mode + if ( line[++char_counter] != 0 ) { return(STATUS_UNSUPPORTED_STATEMENT); } + // Perform reset when toggling off. Check g-code mode should only work if Grbl + // is idle and ready, regardless of alarm locks. This is mainly to keep things + // simple and consistent. + if ( sys.state == STATE_CHECK_MODE ) { + mc_reset(); + report_feedback_message(MESSAGE_DISABLED); } else { - report_startup_line(helper_var,line); + if (sys.state) { return(STATUS_IDLE_ERROR); } + sys.state = STATE_CHECK_MODE; + report_feedback_message(MESSAGE_ENABLED); } - } - break; - } else { // Store startup line - helper_var = true; // Set helper_var to flag storing method. - // No break. Continues into default: to read remaining command characters. - } - default : // Storing setting methods - if(!read_float(line, &char_counter, ¶meter)) { return(STATUS_BAD_NUMBER_FORMAT); } - if(line[char_counter++] != '=') { return(STATUS_UNSUPPORTED_STATEMENT); } - if (helper_var) { // Store startup line - // Prepare sending gcode block to gcode parser by shifting all characters - helper_var = char_counter; // Set helper variable as counter to start of gcode block - do { - line[char_counter-helper_var] = line[char_counter]; - } while (line[char_counter++] != 0); - // Execute gcode block to ensure block is valid. - helper_var = gc_execute_line(line); // Set helper_var to returned status code. - if (helper_var) { return(helper_var); } - else { - helper_var = trunc(parameter); // Set helper_var to int value of parameter - settings_store_startup_line(helper_var,line); - } - } else { // Store global setting. - if(!read_float(line, &char_counter, &value)) { return(STATUS_BAD_NUMBER_FORMAT); } - if(line[char_counter] != 0) { return(STATUS_UNSUPPORTED_STATEMENT); } - return(settings_store_global_setting(parameter, value)); - } + break; + case 'X' : // Disable alarm lock + if ( line[++char_counter] != 0 ) { return(STATUS_UNSUPPORTED_STATEMENT); } + if (sys.state == STATE_ALARM) { + report_feedback_message(MESSAGE_ALARM_UNLOCK); + sys.state = STATE_IDLE; + // Don't run startup script. Prevents stored moves in startup from causing accidents. + } + break; + case 'H' : // Perform homing cycle + if (bit_istrue(settings.flags,BITFLAG_HOMING_ENABLE)) { + // Only perform homing if Grbl is idle or lost. + mc_homing_cycle(); + if (!sys.abort) { protocol_execute_startup(); } // Execute startup scripts after successful homing. + } else { return(STATUS_SETTING_DISABLED); } + break; + case 'N' : // Startup lines. + if ( line[++char_counter] == 0 ) { // Print startup lines + for (helper_var=0; helper_var < N_STARTUP_LINE; helper_var++) { + if (!(settings_read_startup_line(helper_var, line))) { + report_status_message(STATUS_SETTING_READ_FAIL); + } else { + report_startup_line(helper_var,line); + } + } + break; + } else { // Store startup line + helper_var = true; // Set helper_var to flag storing method. + // No break. Continues into default: to read remaining command characters. + } + default : // Storing setting methods + if(!read_float(line, &char_counter, ¶meter)) { return(STATUS_BAD_NUMBER_FORMAT); } + if(line[char_counter++] != '=') { return(STATUS_UNSUPPORTED_STATEMENT); } + if (helper_var) { // Store startup line + // Prepare sending gcode block to gcode parser by shifting all characters + helper_var = char_counter; // Set helper variable as counter to start of gcode block + do { + line[char_counter-helper_var] = line[char_counter]; + } while (line[char_counter++] != 0); + // Execute gcode block to ensure block is valid. + helper_var = gc_execute_line(line); // Set helper_var to returned status code. + if (helper_var) { return(helper_var); } + else { + helper_var = trunc(parameter); // Set helper_var to int value of parameter + settings_store_startup_line(helper_var,line); + } + } else { // Store global setting. + if(!read_float(line, &char_counter, &value)) { return(STATUS_BAD_NUMBER_FORMAT); } + if(line[char_counter] != 0) { return(STATUS_UNSUPPORTED_STATEMENT); } + return(settings_store_global_setting(parameter, value)); + } + } } return(STATUS_OK); // If '$' command makes it to here, then everything's ok. diff --git a/report.c b/report.c index c44efe0..49e6501 100644 --- a/report.c +++ b/report.c @@ -71,7 +71,7 @@ void report_status_message(uint8_t status_code) case STATUS_SETTING_READ_FAIL: printPgmString(PSTR("EEPROM read fail. Using defaults")); break; case STATUS_IDLE_ERROR: - printPgmString(PSTR("Busy or queued")); break; + printPgmString(PSTR("Not idle")); break; case STATUS_ALARM_LOCK: printPgmString(PSTR("Alarm lock")); break; case STATUS_SOFT_LIMIT_ERROR: @@ -162,24 +162,27 @@ void report_grbl_settings() { printPgmString(PSTR(" (y max travel, mm)\r\n$11=")); printFloat(-settings.max_travel[Z_AXIS]); // Grbl internally store this as negative. printPgmString(PSTR(" (z max travel, mm)\r\n$12=")); printInteger(settings.pulse_microseconds); printPgmString(PSTR(" (step pulse, usec)\r\n$13=")); printFloat(settings.default_feed_rate); - printPgmString(PSTR(" (default feed, mm/min)\r\n$14=")); printInteger(settings.invert_mask); - printPgmString(PSTR(" (step port invert mask, int:")); print_uint8_base2(settings.invert_mask); - printPgmString(PSTR(")\r\n$15=")); printInteger(settings.stepper_idle_lock_time); - printPgmString(PSTR(" (step idle delay, msec)\r\n$16=")); printFloat(settings.junction_deviation); - printPgmString(PSTR(" (junction deviation, mm)\r\n$17=")); printFloat(settings.arc_tolerance); - printPgmString(PSTR(" (arc tolerance, mm)\r\n$18=")); printInteger(settings.decimal_places); - printPgmString(PSTR(" (n-decimals, int)\r\n$19=")); printInteger(bit_istrue(settings.flags,BITFLAG_REPORT_INCHES)); - printPgmString(PSTR(" (report inches, bool)\r\n$20=")); printInteger(bit_istrue(settings.flags,BITFLAG_AUTO_START)); - printPgmString(PSTR(" (auto start, bool)\r\n$21=")); printInteger(bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)); - printPgmString(PSTR(" (invert step enable, bool)\r\n$22=")); printInteger(bit_istrue(settings.flags,BITFLAG_SOFT_LIMIT_ENABLE)); - printPgmString(PSTR(" (soft limits, bool)\r\n$23=")); printInteger(bit_istrue(settings.flags,BITFLAG_HARD_LIMIT_ENABLE)); - printPgmString(PSTR(" (hard limits, bool)\r\n$24=")); printInteger(bit_istrue(settings.flags,BITFLAG_HOMING_ENABLE)); - printPgmString(PSTR(" (homing cycle, bool)\r\n$25=")); printInteger(settings.homing_dir_mask); + printPgmString(PSTR(" (default feed, mm/min)\r\n$14=")); printInteger(settings.step_invert_mask); + printPgmString(PSTR(" (step port invert mask, int:")); print_uint8_base2(settings.step_invert_mask); + printPgmString(PSTR(")\r\n$15=")); printInteger(settings.dir_invert_mask); + printPgmString(PSTR(" (dir port invert mask, int:")); print_uint8_base2(settings.dir_invert_mask); + printPgmString(PSTR(")\r\n$16=")); printInteger(settings.stepper_idle_lock_time); + printPgmString(PSTR(" (step idle delay, msec)\r\n$17=")); printFloat(settings.junction_deviation); + printPgmString(PSTR(" (junction deviation, mm)\r\n$18=")); printFloat(settings.arc_tolerance); + printPgmString(PSTR(" (arc tolerance, mm)\r\n$19=")); printInteger(settings.decimal_places); + printPgmString(PSTR(" (n-decimals, int)\r\n$20=")); printInteger(bit_istrue(settings.flags,BITFLAG_REPORT_INCHES)); + printPgmString(PSTR(" (report inches, bool)\r\n$21=")); printInteger(bit_istrue(settings.flags,BITFLAG_AUTO_START)); + printPgmString(PSTR(" (auto start, bool)\r\n$22=")); printInteger(bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)); + printPgmString(PSTR(" (invert step enable, bool)\r\n$23=")); printInteger(bit_istrue(settings.flags,BITFLAG_INVERT_LIMIT_PINS)); + printPgmString(PSTR(" (invert limit pins, bool)\r\n$24=")); printInteger(bit_istrue(settings.flags,BITFLAG_SOFT_LIMIT_ENABLE)); + printPgmString(PSTR(" (soft limits, bool)\r\n$25=")); printInteger(bit_istrue(settings.flags,BITFLAG_HARD_LIMIT_ENABLE)); + printPgmString(PSTR(" (hard limits, bool)\r\n$26=")); printInteger(bit_istrue(settings.flags,BITFLAG_HOMING_ENABLE)); + printPgmString(PSTR(" (homing cycle, bool)\r\n$27=")); printInteger(settings.homing_dir_mask); printPgmString(PSTR(" (homing dir invert mask, int:")); print_uint8_base2(settings.homing_dir_mask); - printPgmString(PSTR(")\r\n$26=")); printFloat(settings.homing_feed_rate); - printPgmString(PSTR(" (homing feed, mm/min)\r\n$27=")); printFloat(settings.homing_seek_rate); - printPgmString(PSTR(" (homing seek, mm/min)\r\n$28=")); printInteger(settings.homing_debounce_delay); - printPgmString(PSTR(" (homing debounce, msec)\r\n$29=")); printFloat(settings.homing_pulloff); + printPgmString(PSTR(")\r\n$28=")); printFloat(settings.homing_feed_rate); + printPgmString(PSTR(" (homing feed, mm/min)\r\n$29=")); printFloat(settings.homing_seek_rate); + printPgmString(PSTR(" (homing seek, mm/min)\r\n$30=")); printInteger(settings.homing_debounce_delay); + printPgmString(PSTR(" (homing debounce, msec)\r\n$31=")); printFloat(settings.homing_pulloff); printPgmString(PSTR(" (homing pull-off, mm)\r\n")); } diff --git a/settings.c b/settings.c index a611ccc..1ba0629 100644 --- a/settings.c +++ b/settings.c @@ -80,7 +80,8 @@ void settings_reset(bool reset_all) { settings.acceleration[Y_AXIS] = DEFAULT_Y_ACCELERATION; settings.acceleration[Z_AXIS] = DEFAULT_Z_ACCELERATION; settings.arc_tolerance = DEFAULT_ARC_TOLERANCE; - settings.invert_mask = DEFAULT_STEPPING_INVERT_MASK; + settings.step_invert_mask = DEFAULT_STEPPING_INVERT_MASK; + settings.dir_invert_mask = DEFAULT_DIRECTION_INVERT_MASK; settings.junction_deviation = DEFAULT_JUNCTION_DEVIATION; } // New settings since last version @@ -88,6 +89,7 @@ void settings_reset(bool reset_all) { if (DEFAULT_REPORT_INCHES) { settings.flags |= BITFLAG_REPORT_INCHES; } if (DEFAULT_AUTO_START) { settings.flags |= BITFLAG_AUTO_START; } if (DEFAULT_INVERT_ST_ENABLE) { settings.flags |= BITFLAG_INVERT_ST_ENABLE; } + if (DEFAULT_INVERT_LIMIT_PINS) { settings.flags |= BITFLAG_INVERT_LIMIT_PINS; } if (DEFAULT_SOFT_LIMIT_ENABLE) { settings.flags |= BITFLAG_SOFT_LIMIT_ENABLE; } if (DEFAULT_HARD_LIMIT_ENABLE) { settings.flags |= BITFLAG_HARD_LIMIT_ENABLE; } if (DEFAULT_HOMING_ENABLE) { settings.flags |= BITFLAG_HOMING_ENABLE; } @@ -176,46 +178,51 @@ uint8_t settings_store_global_setting(int parameter, float value) { if (value < 3) { return(STATUS_SETTING_STEP_PULSE_MIN); } settings.pulse_microseconds = round(value); break; case 13: settings.default_feed_rate = value; break; - case 14: settings.invert_mask = trunc(value); break; - case 15: settings.stepper_idle_lock_time = round(value); break; - case 16: settings.junction_deviation = fabs(value); break; - case 17: settings.arc_tolerance = value; break; - case 18: settings.decimal_places = round(value); break; - case 19: + case 14: settings.step_invert_mask = trunc(value); break; + case 15: settings.dir_invert_mask = trunc(value); break; + case 16: settings.stepper_idle_lock_time = round(value); break; + case 17: settings.junction_deviation = fabs(value); break; + case 18: settings.arc_tolerance = value; break; + case 19: settings.decimal_places = round(value); break; + case 20: if (value) { settings.flags |= BITFLAG_REPORT_INCHES; } else { settings.flags &= ~BITFLAG_REPORT_INCHES; } break; - case 20: // Reset to ensure change. Immediate re-init may cause problems. + case 21: // Reset to ensure change. Immediate re-init may cause problems. if (value) { settings.flags |= BITFLAG_AUTO_START; } else { settings.flags &= ~BITFLAG_AUTO_START; } break; - case 21: // Reset to ensure change. Immediate re-init may cause problems. + case 22: // Reset to ensure change. Immediate re-init may cause problems. if (value) { settings.flags |= BITFLAG_INVERT_ST_ENABLE; } else { settings.flags &= ~BITFLAG_INVERT_ST_ENABLE; } break; - case 22: + case 23: // Reset to ensure change. Immediate re-init may cause problems. + if (value) { settings.flags |= BITFLAG_INVERT_LIMIT_PINS; } + else { settings.flags &= ~BITFLAG_INVERT_LIMIT_PINS; } + break; + case 24: if (value) { if (bit_isfalse(settings.flags, BITFLAG_HOMING_ENABLE)) { return(STATUS_SOFT_LIMIT_ERROR); } settings.flags |= BITFLAG_SOFT_LIMIT_ENABLE; } else { settings.flags &= ~BITFLAG_SOFT_LIMIT_ENABLE; } break; - case 23: + case 25: if (value) { settings.flags |= BITFLAG_HARD_LIMIT_ENABLE; } else { settings.flags &= ~BITFLAG_HARD_LIMIT_ENABLE; } limits_init(); // Re-init to immediately change. NOTE: Nice to have but could be problematic later. break; - case 24: + case 26: if (value) { settings.flags |= BITFLAG_HOMING_ENABLE; } else { settings.flags &= ~BITFLAG_HOMING_ENABLE; - settings.flags &= ~BITFLAG_SOFT_LIMIT_ENABLE; + settings.flags &= ~BITFLAG_SOFT_LIMIT_ENABLE; // Force disable soft-limits. } break; - case 25: settings.homing_dir_mask = trunc(value); break; - case 26: settings.homing_feed_rate = value; break; - case 27: settings.homing_seek_rate = value; break; - case 28: settings.homing_debounce_delay = round(value); break; - case 29: settings.homing_pulloff = value; break; + case 27: settings.homing_dir_mask = trunc(value); break; + case 28: settings.homing_feed_rate = value; break; + case 29: settings.homing_seek_rate = value; break; + case 30: settings.homing_debounce_delay = round(value); break; + case 31: settings.homing_pulloff = value; break; default: return(STATUS_INVALID_STATEMENT); } diff --git a/settings.h b/settings.h index ebd7db5..7d435d9 100644 --- a/settings.h +++ b/settings.h @@ -30,7 +30,7 @@ // Version of the EEPROM data. Will be used to migrate existing data from older versions of Grbl // when firmware is upgraded. Always stored in byte 0 of eeprom -#define SETTINGS_VERSION 56 +#define SETTINGS_VERSION 57 // Define bit flag masks for the boolean settings in settings.flag. #define BITFLAG_REPORT_INCHES bit(0) @@ -39,6 +39,7 @@ #define BITFLAG_HARD_LIMIT_ENABLE bit(3) #define BITFLAG_HOMING_ENABLE bit(4) #define BITFLAG_SOFT_LIMIT_ENABLE bit(5) +#define BITFLAG_INVERT_LIMIT_PINS bit(6) // Define EEPROM memory address location values for Grbl settings and parameters // NOTE: The Atmega328p has 1KB EEPROM. The upper half is reserved for parameters and @@ -64,7 +65,8 @@ typedef struct { float max_travel[N_AXIS]; uint8_t pulse_microseconds; float default_feed_rate; - uint8_t invert_mask; + uint8_t step_invert_mask; + uint8_t dir_invert_mask; uint8_t stepper_idle_lock_time; // If max value 255, steppers do not disable. float junction_deviation; float arc_tolerance; diff --git a/stepper.c b/stepper.c index b02c9d5..b9a80f9 100644 --- a/stepper.c +++ b/stepper.c @@ -35,6 +35,12 @@ #define RAMP_DECEL 2 +#define MAX_AMASS_LEVEL 3 +#define AMASS_LEVEL1 (F_CPU/10000) +#define AMASS_LEVEL2 (F_CPU/5000) +#define AMASS_LEVEL3 (F_CPU/2500) + + // Stores the planner block Bresenham algorithm execution data for the segments in the segment // buffer. Normally, this buffer is partially in-use, but, for the worst case scenario, it will // never exceed the number of accessible stepper buffer segments (SEGMENT_BUFFER_SIZE-1). @@ -57,7 +63,7 @@ typedef struct { uint16_t n_step; // Number of step events to be executed for this segment uint8_t st_block_index; // Stepper block data index. Uses this information to execute this segment. uint16_t cycles_per_tick; // Step distance traveled per ISR tick, aka step rate. - uint8_t prescaler; + uint8_t amass_level; } segment_t; static segment_t segment_buffer[SEGMENT_BUFFER_SIZE]; @@ -65,17 +71,19 @@ static segment_t segment_buffer[SEGMENT_BUFFER_SIZE]; typedef struct { // Used by the bresenham line algorithm uint32_t counter_x, // Counter variables for the bresenham line tracer - counter_y, - counter_z; + counter_y, + counter_z; - #if STEP_PULSE_DELAY > 0 + #ifdef STEP_PULSE_DELAY uint8_t step_bits; // Stores out_bits output to complete the step pulse delay #endif // Used by the stepper driver interrupt uint8_t execute_step; // Flags step execution for each interrupt. uint8_t step_pulse_time; // Step pulse reset time after step rise - uint8_t out_bits; // The next stepping-bits to be output + uint8_t step_outbits; // The next stepping-bits to be output + uint8_t dir_outbits; + uint32_t steps[N_AXIS]; uint16_t step_count; // Steps remaining in line segment motion uint8_t exec_block_index; // Tracks the current st_block index. Change indicates new block. @@ -120,6 +128,8 @@ typedef struct { static st_prep_t prep; +static void st_config_step_timer(uint32_t cycles); + /* BLOCK VELOCITY PROFILE DEFINITION __________________________ /| |\ _________________ ^ @@ -158,6 +168,7 @@ static st_prep_t prep; are shown and defined in the above illustration. */ + // Stepper state initialization. Cycle should only start if the st.cycle_start flag is // enabled. Startup init and limits call this function but shouldn't start the cycle. void st_wake_up() @@ -170,20 +181,22 @@ void st_wake_up() } if (sys.state & (STATE_CYCLE | STATE_HOMING)){ // Initialize stepper output bits - st.out_bits = settings.invert_mask; - + st.dir_outbits = settings.dir_invert_mask; + st.step_outbits = settings.step_invert_mask; + // Initialize step pulse timing from settings. Here to ensure updating after re-writing. #ifdef STEP_PULSE_DELAY // Set total step pulse time after direction pin set. Ad hoc computation from oscilloscope. st.step_pulse_time = -(((settings.pulse_microseconds+STEP_PULSE_DELAY-2)*TICKS_PER_MICROSECOND) >> 3); // Set delay between direction pin write and step command. - OCR2A = -(((settings.pulse_microseconds)*TICKS_PER_MICROSECOND) >> 3); + OCR0A = -(((settings.pulse_microseconds)*TICKS_PER_MICROSECOND) >> 3); #else // Normal operation // Set step pulse time. Ad hoc computation from oscilloscope. Uses two's complement. st.step_pulse_time = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3); #endif - // Enable stepper driver interrupt + // Enable Stepper Driver Interrupt + TCCR1B = (TCCR1B & ~(0x07<prescaler<prescaler<cycles_per_tick; st.step_count = st.exec_segment->n_step; // NOTE: Can sometimes be zero when moving slow. // If the new segment starts a new planner block, initialize stepper variables and counters. @@ -274,11 +286,15 @@ ISR(TIMER1_COMPA_vect) if ( st.exec_block_index != st.exec_segment->st_block_index ) { st.exec_block_index = st.exec_segment->st_block_index; st.exec_block = &st_block_buffer[st.exec_block_index]; + st.dir_outbits = st.exec_block->direction_bits ^ settings.dir_invert_mask; // Initialize Bresenham line and distance counters st.counter_x = (st.exec_block->step_event_count >> 1); st.counter_y = st.counter_x; - st.counter_z = st.counter_x; + st.counter_z = st.counter_x; } + st.steps[X_AXIS] = st.exec_block->steps[X_AXIS] >> st.exec_segment->amass_level; + st.steps[Y_AXIS] = st.exec_block->steps[Y_AXIS] >> st.exec_segment->amass_level; + st.steps[Z_AXIS] = st.exec_block->steps[Z_AXIS] >> st.exec_segment->amass_level; } else { // Segment buffer empty. Shutdown. st_go_idle(); @@ -287,46 +303,43 @@ ISR(TIMER1_COMPA_vect) } } - // Reset out_bits and reload direction bits - st.out_bits = st.exec_block->direction_bits; + // Reset step out bits. + st.step_outbits = 0; // Execute step displacement profile by Bresenham line algorithm - if (st.step_count > 0) { - st.step_count--; // Decrement step events count + st.counter_x += st.steps[X_AXIS]; + if (st.counter_x > st.exec_block->step_event_count) { + st.step_outbits |= (1<step_event_count; + if (st.exec_block->direction_bits & (1< st.exec_block->step_event_count) { + st.step_outbits |= (1<step_event_count; + if (st.exec_block->direction_bits & (1< st.exec_block->step_event_count) { + st.step_outbits |= (1<step_event_count; + if (st.exec_block->direction_bits & (1<steps[X_AXIS]; - if (st.counter_x > st.exec_block->step_event_count) { - st.out_bits |= (1<step_event_count; - if (st.out_bits & (1<steps[Y_AXIS]; - if (st.counter_y > st.exec_block->step_event_count) { - st.out_bits |= (1<step_event_count; - if (st.out_bits & (1<steps[Z_AXIS]; - if (st.counter_z > st.exec_block->step_event_count) { - st.out_bits |= (1<step_event_count; - if (st.out_bits & (1<direction_bits = pl_block->direction_bits; - st_prep_block->steps[X_AXIS] = pl_block->steps[X_AXIS]; - st_prep_block->steps[Y_AXIS] = pl_block->steps[Y_AXIS]; - st_prep_block->steps[Z_AXIS] = pl_block->steps[Z_AXIS]; - st_prep_block->step_event_count = pl_block->step_event_count; + #ifdef ACTIVE_MULTI_AXIS_STEP_SMOOTHING + st_prep_block->steps[X_AXIS] = pl_block->steps[X_AXIS] << MAX_AMASS_LEVEL; + st_prep_block->steps[Y_AXIS] = pl_block->steps[Y_AXIS] << MAX_AMASS_LEVEL; + st_prep_block->steps[Z_AXIS] = pl_block->steps[Z_AXIS] << MAX_AMASS_LEVEL; + st_prep_block->step_event_count = pl_block->step_event_count << MAX_AMASS_LEVEL; + #else + st_prep_block->steps[X_AXIS] = pl_block->steps[X_AXIS]; + st_prep_block->steps[Y_AXIS] = pl_block->steps[Y_AXIS]; + st_prep_block->steps[Z_AXIS] = pl_block->steps[Z_AXIS]; + st_prep_block->step_event_count = pl_block->step_event_count; + #endif // Initialize segment buffer data for generating the segments. prep.steps_remaining = pl_block->step_event_count; @@ -678,11 +695,26 @@ void st_prep_buffer() uint32_t cycles; float steps_remaining = prep.step_per_mm*mm_remaining; - float inv_rate = dt/(prep.steps_remaining-steps_remaining); - cycles = ceil( (TICKS_PER_MICROSECOND*1000000*60)*inv_rate ); // (ftol_mult*step/isr_tic) - // Compute number of steps to execute and segment step phase correction. - prep_segment->n_step = ceil(prep.steps_remaining)-ceil(steps_remaining); + prep_segment->n_step = ceil(prep.steps_remaining)-ceil(steps_remaining); + + float inv_rate = dt/(prep.steps_remaining-steps_remaining); + cycles = ceil( (TICKS_PER_MICROSECOND*1000000*60)*inv_rate ); // (cycles/step) + + #ifdef ACTIVE_MULTI_AXIS_STEP_SMOOTHING + // Compute step timing and multi-axis smoothing level. + // NOTE: Only one prescalar is required with AMASS enabled. + if (cycles > AMASS_LEVEL1) { + if (cycles > AMASS_LEVEL2) { + if (cycles > AMASS_LEVEL3) { prep_segment->amass_level = 3; } + else { prep_segment->amass_level = 2; } + } else { prep_segment->amass_level = 1; } + cycles >>= prep_segment->amass_level; + prep_segment->n_step <<= prep_segment->amass_level; + } else { prep_segment->amass_level = 0; } + if (cycles < (1UL << 16)) { prep_segment->cycles_per_tick = cycles; } + else { prep_segment->cycles_per_tick = 0xffff; } // Just set the slowest speed possible. (4.1ms @ 16MHz) + #endif // Determine end of segment conditions. Setup initial conditions for next segment. if (mm_remaining > prep.mm_complete) { @@ -715,29 +747,6 @@ void st_prep_buffer() } } -// TODO: Compile-time checks to what prescalers need to be compiled in. - if (cycles < (1UL << 16)) { // < 65536 (4.1ms @ 16MHz) - prep_segment->prescaler = 1; // prescaler: 0 - prep_segment->cycles_per_tick = cycles; - } else { - prep_segment->prescaler = 2; // prescaler: 8 - if (cycles < (1UL << 19)) { // < 524288 (32.8ms@16MHz) - prep_segment->cycles_per_tick = cycles >> 3; -// } else if (cycles < (1UL << 22)) { // < 4194304 (262ms@16MHz) -// prep_segment->prescaler = 3; // prescaler: 64 -// prep_segment->cycles_per_tick = cycles >> 6; -// } else if (cycles < (1UL << 24)) { // < 16777216 (1.05sec@16MHz) -// prep_segment->prescaler = 4; // prescaler: 256 -// prep_segment->cycles_per_tick = (cycles >> 8); -// } else { -// prep_segment->prescaler = 5; // prescaler: 1024 -// if (cycles < (1UL << 26)) { // < 67108864 (4.19sec@16MHz) -// prep_segment->cycles_per_tick = (cycles >> 10); - } else { // Just set the slowest speed possible. - prep_segment->cycles_per_tick = 0xffff; - } - } - // New step segment initialization completed. Increment segment buffer indices. segment_buffer_head = segment_next_head; if ( ++segment_next_head == SEGMENT_BUFFER_SIZE ) { segment_next_head = 0; } @@ -776,3 +785,31 @@ void st_prep_buffer() we know when the plan is feasible in the context of what's already in the code and not require too much more code? */ + +// static void st_config_step_timer(uint32_t cycles) +// { +// if (cycles < (1UL << 16)) { // < 65536 (4.1ms @ 16MHz) +// prep_segment->prescaler = 1; // prescaler: 0 +// prep_segment->cycles_per_tick = cycles; +// } else { +// prep_segment->prescaler = 2; // prescaler: 8 +// if (cycles < (1UL << 19)) { // < 524288 (32.8ms@16MHz) +// prep_segment->cycles_per_tick = cycles >> 3; +// +// // } else if (cycles < (1UL << 22)) { // < 4194304 (262ms@16MHz) +// // prep_segment->prescaler = 3; // prescaler: 64 +// // prep_segment->cycles_per_tick = cycles >> 6; +// // } else if (cycles < (1UL << 24)) { // < 16777216 (1.05sec@16MHz) +// // prep_segment->prescaler = 4; // prescaler: 256 +// // prep_segment->cycles_per_tick = (cycles >> 8); +// // } else { +// // prep_segment->prescaler = 5; // prescaler: 1024 +// // if (cycles < (1UL << 26)) { // < 67108864 (4.19sec@16MHz) +// // prep_segment->cycles_per_tick = (cycles >> 10); +// +// } else { // Just set the slowest speed possible. +// prep_segment->cycles_per_tick = 0xffff; +// } +// printString("X"); +// } +// } \ No newline at end of file From f10bad43b2c5b2324e3b2ebf74b58640078107f5 Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Mon, 30 Dec 2013 22:02:05 -0700 Subject: [PATCH 31/73] Minor bug fixes: Homing travel calculations. Cycle resuming after spindle and dwell commands. - Homing travel calculations fixed. It was computing the min travel rather than max. - Auto-start disable and pausing after spindle or dwell commands. Related to plan_synchronize() function call. Now fixed, but still need to work on the system state. - Pushed a fix to make this branch more Arduino IDE compatible. Removed extern call in nuts_bolts.c - Updated the stepper configuration option of enabling or disabling the new Adaptive Multi-Axis Step Smoothing Algorithm. Now works either way. - Updated some copyright info. --- config.h | 4 +- coolant_control.c | 2 +- coolant_control.h | 2 +- cpu_map.h | 2 +- defaults.h | 2 +- gcode.c | 2 +- gcode.h | 2 +- limits.c | 13 +++--- limits.h | 2 +- main.c | 4 +- motion_control.c | 10 +--- motion_control.h | 2 +- nuts_bolts.c | 5 +- nuts_bolts.h | 2 +- planner.c | 5 +- planner.h | 2 +- print.c | 2 +- print.h | 2 +- protocol.c | 2 +- protocol.h | 2 +- report.c | 2 +- report.h | 2 +- serial.c | 2 +- serial.h | 2 +- settings.c | 2 +- settings.h | 2 +- spindle_control.c | 2 +- spindle_control.h | 2 +- stepper.c | 113 +++++++++++++++++++++++++--------------------- stepper.h | 2 +- 30 files changed, 102 insertions(+), 98 deletions(-) diff --git a/config.h b/config.h index e46aa81..daa6268 100644 --- a/config.h +++ b/config.h @@ -2,7 +2,7 @@ config.h - compile time configuration Part of Grbl - Copyright (c) 2011-2013 Sungeun K. Jeon + Copyright (c) 2011-2014 Sungeun K. Jeon Copyright (c) 2009-2011 Simen Svale Skogsrud Grbl is free software: you can redistribute it and/or modify @@ -88,7 +88,7 @@ // set this only as high as needed. Approximate successful values can widely range from 50 to 200 or more. #define ACCELERATION_TICKS_PER_SECOND 100 -#define ACTIVE_MULTI_AXIS_STEP_SMOOTHING +#define ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING #define ENABLE_SOFTWARE_DEBOUNCE // Minimum planner junction speed. Sets the default minimum junction speed the planner plans to at diff --git a/coolant_control.c b/coolant_control.c index 8abd674..b866aef 100644 --- a/coolant_control.c +++ b/coolant_control.c @@ -2,7 +2,7 @@ coolant_control.c - coolant control methods Part of Grbl - Copyright (c) 2012 Sungeun K. Jeon + Copyright (c) 2012-2014 Sungeun K. 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 diff --git a/coolant_control.h b/coolant_control.h index fd2d549..38f3b43 100644 --- a/coolant_control.h +++ b/coolant_control.h @@ -2,7 +2,7 @@ coolant_control.h - spindle control methods Part of Grbl - Copyright (c) 2012 Sungeun K. Jeon + Copyright (c) 2012-2014 Sungeun K. 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 diff --git a/cpu_map.h b/cpu_map.h index 0d21fe3..ed97275 100644 --- a/cpu_map.h +++ b/cpu_map.h @@ -2,7 +2,7 @@ cpu_map.h - CPU and pin mapping configuration file Part of Grbl - Copyright (c) 2013 Sungeun K. Jeon + Copyright (c) 2013-2014 Sungeun K. 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 diff --git a/defaults.h b/defaults.h index 4048511..cbee821 100644 --- a/defaults.h +++ b/defaults.h @@ -2,7 +2,7 @@ defaults.h - defaults settings configuration file Part of Grbl - Copyright (c) 2012-2013 Sungeun K. Jeon + Copyright (c) 2012-2014 Sungeun K. 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 diff --git a/gcode.c b/gcode.c index 0900dec..d8abaf6 100644 --- a/gcode.c +++ b/gcode.c @@ -2,8 +2,8 @@ gcode.c - rs274/ngc parser. Part of Grbl + Copyright (c) 2011-2014 Sungeun K. Jeon Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2011-2013 Sungeun K. 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 diff --git a/gcode.h b/gcode.h index 6b8949a..8c20957 100644 --- a/gcode.h +++ b/gcode.h @@ -2,8 +2,8 @@ gcode.h - rs274/ngc parser. Part of Grbl + Copyright (c) 2011-2014 Sungeun K. Jeon Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2011-2012 Sungeun K. 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 diff --git a/limits.c b/limits.c index 43134e2..a9e15f2 100644 --- a/limits.c +++ b/limits.c @@ -2,8 +2,8 @@ limits.c - code pertaining to limit-switches and performing the homing cycle Part of Grbl + Copyright (c) 2012-2014 Sungeun K. Jeon Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2012-2013 Sungeun K. 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 @@ -132,12 +132,13 @@ void limits_go_home(uint8_t cycle_mask, bool approach, float homing_rate) else { invert_pin = !approach; } // Determine travel distance to the furthest homing switch based on user max travel settings. + // NOTE: settings.max_travel[] is stored as a negative value. float max_travel = settings.max_travel[X_AXIS]; - if (max_travel < settings.max_travel[Y_AXIS]) { max_travel = settings.max_travel[Y_AXIS]; } - if (max_travel < settings.max_travel[Z_AXIS]) { max_travel = settings.max_travel[Z_AXIS]; } - max_travel *= 1.25; // Ensure homing switches engaged by over-estimating max travel. - if (approach) { max_travel = -max_travel; } - + if (max_travel > settings.max_travel[Y_AXIS]) { max_travel = settings.max_travel[Y_AXIS]; } + if (max_travel > settings.max_travel[Z_AXIS]) { max_travel = settings.max_travel[Z_AXIS]; } + max_travel *= -1.25; // Ensure homing switches engaged by over-estimating max travel. + if (!approach) { max_travel = -max_travel; } + // Set target location and rate for active axes. float target[N_AXIS]; uint8_t n_active_axis = 0; diff --git a/limits.h b/limits.h index 9c414a4..9e6fdb9 100644 --- a/limits.h +++ b/limits.h @@ -2,8 +2,8 @@ limits.h - code pertaining to limit-switches and performing the homing cycle Part of Grbl + Copyright (c) 2013-2014 Sungeun K. Jeon Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2013 Sungeun K. 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 diff --git a/main.c b/main.c index 3e279e0..d347438 100644 --- a/main.c +++ b/main.c @@ -1,9 +1,9 @@ /* main.c - An embedded CNC Controller with rs274/ngc (g-code) support Part of Grbl - + + Copyright (c) 2011-2014 Sungeun K. Jeon Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2011-2013 Sungeun K. 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 diff --git a/motion_control.c b/motion_control.c index 94398b7..e30274a 100644 --- a/motion_control.c +++ b/motion_control.c @@ -2,8 +2,8 @@ motion_control.c - high level interface for issuing motion commands Part of Grbl + Copyright (c) 2011-2014 Sungeun K. Jeon Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2011-2013 Sungeun K. Jeon Copyright (c) 2011 Jens Geisler Grbl is free software: you can redistribute it and/or modify @@ -301,13 +301,7 @@ void mc_homing_cycle() // NOTE: This function is called from the main loop and mc_line() only and executes when one of // two conditions exist respectively: There are no more blocks sent (i.e. streaming is finished, // single commands), or the planner buffer is full and ready to go. -void mc_auto_cycle_start() -{ - if (sys.auto_start) { - st_cycle_start(); - if (bit_isfalse(settings.flags,BITFLAG_AUTO_START)) { sys.auto_start = false; } // Reset auto start per settings. - } -} +void mc_auto_cycle_start() { if (sys.auto_start) { st_cycle_start(); } } // Method to ready the system to reset by setting the runtime reset command and killing any diff --git a/motion_control.h b/motion_control.h index 3ca8d30..a5e0371 100644 --- a/motion_control.h +++ b/motion_control.h @@ -2,8 +2,8 @@ motion_control.h - high level interface for issuing motion commands Part of Grbl + Copyright (c) 2011-2014 Sungeun K. Jeon Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2011-2013 Sungeun K. 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 diff --git a/nuts_bolts.c b/nuts_bolts.c index 703c1db..6914052 100644 --- a/nuts_bolts.c +++ b/nuts_bolts.c @@ -2,8 +2,8 @@ nuts_bolts.c - Shared functions Part of Grbl + Copyright (c) 2011-2014 Sungeun K. Jeon Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2011-2012 Sungeun K. 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 @@ -25,7 +25,6 @@ #include "planner.h" #define MAX_INT_DIGITS 8 // Maximum number of digits in int32 (and float) -extern float __floatunsisf (unsigned long); // Extracts a floating point value from a string. The following code is based loosely on // the avr-libc strtod() function by Michael Stumpf and Dmitry Xmelkov and many freely @@ -79,7 +78,7 @@ int read_float(char *line, uint8_t *char_counter, float *float_ptr) // Convert integer into floating point. float fval; - fval = __floatunsisf(intval); + fval = (float)intval; // Apply decimal. Should perform no more than two floating point multiplications for the // expected range of E0 to E-4. diff --git a/nuts_bolts.h b/nuts_bolts.h index ab19485..5140575 100644 --- a/nuts_bolts.h +++ b/nuts_bolts.h @@ -2,8 +2,8 @@ nuts_bolts.h - Header file for shared definitions, variables, and functions Part of Grbl + Copyright (c) 2011-2014 Sungeun K. Jeon Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2011-2013 Sungeun K. 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 diff --git a/planner.c b/planner.c index 09a2c44..143837a 100644 --- a/planner.c +++ b/planner.c @@ -2,7 +2,7 @@ planner.c - buffers movement commands and manages the acceleration profile plan Part of Grbl - Copyright (c) 2011-2013 Sungeun K. Jeon + Copyright (c) 2011-2014 Sungeun K. Jeon Copyright (c) 2009-2011 Simen Svale Skogsrud Copyright (c) 2011 Jens Geisler @@ -255,7 +255,8 @@ uint8_t plan_check_full_buffer() // during a synchronize call, if it should happen. Also, waits for clean cycle end. void plan_synchronize() { - sys.auto_start = true; // Set auto start to resume cycle after synchronize and caller completes. + // Check and set auto start to resume cycle after synchronize and caller completes. + if (sys.state == STATE_CYCLE) { sys.auto_start = true; } while (plan_get_current_block() || (sys.state == STATE_CYCLE)) { protocol_execute_runtime(); // Check and execute run-time commands if (sys.abort) { return; } // Check for system abort diff --git a/planner.h b/planner.h index 3ec3bea..1cc4309 100644 --- a/planner.h +++ b/planner.h @@ -2,7 +2,7 @@ planner.h - buffers movement commands and manages the acceleration profile plan Part of Grbl - Copyright (c) 2011-2013 Sungeun K. Jeon + Copyright (c) 2011-2014 Sungeun K. Jeon Copyright (c) 2009-2011 Simen Svale Skogsrud Grbl is free software: you can redistribute it and/or modify diff --git a/print.c b/print.c index 7a7686b..0e2f9b8 100644 --- a/print.c +++ b/print.c @@ -2,8 +2,8 @@ print.c - Functions for formatting output strings Part of Grbl + Copyright (c) 2011-2014 Sungeun K. Jeon Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2011-2012 Sungeun K. 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 diff --git a/print.h b/print.h index 8a161c1..ecd4238 100644 --- a/print.h +++ b/print.h @@ -2,8 +2,8 @@ print.h - Functions for formatting output strings Part of Grbl + Copyright (c) 2011-2014 Sungeun K. Jeon Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2011-2012 Sungeun K. 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 diff --git a/protocol.c b/protocol.c index 1e8e9cb..29d81e6 100644 --- a/protocol.c +++ b/protocol.c @@ -2,8 +2,8 @@ protocol.c - the serial protocol master control unit Part of Grbl + Copyright (c) 2011-2014 Sungeun K. Jeon Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2011-2013 Sungeun K. 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 diff --git a/protocol.h b/protocol.h index 4d90c1c..b6267d4 100644 --- a/protocol.h +++ b/protocol.h @@ -2,8 +2,8 @@ protocol.h - the serial protocol master control unit Part of Grbl + Copyright (c) 2011-2014 Sungeun K. Jeon Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2011-2012 Sungeun K. 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 diff --git a/report.c b/report.c index 49e6501..7e32ef1 100644 --- a/report.c +++ b/report.c @@ -2,7 +2,7 @@ report.c - reporting and messaging methods Part of Grbl - Copyright (c) 2012-2013 Sungeun K. Jeon + Copyright (c) 2012-2014 Sungeun K. 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 diff --git a/report.h b/report.h index d9fd5b7..53c97dc 100644 --- a/report.h +++ b/report.h @@ -2,7 +2,7 @@ report.h - reporting and messaging methods Part of Grbl - Copyright (c) 2012-2013 Sungeun K. Jeon + Copyright (c) 2012-2014 Sungeun K. 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 diff --git a/serial.c b/serial.c index c07e64b..382ee41 100644 --- a/serial.c +++ b/serial.c @@ -2,8 +2,8 @@ serial.c - Low level functions for sending and recieving bytes via the serial port Part of Grbl + Copyright (c) 2011-2014 Sungeun K. Jeon Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2011-2012 Sungeun K. 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 diff --git a/serial.h b/serial.h index 723d48f..e768b44 100644 --- a/serial.h +++ b/serial.h @@ -2,8 +2,8 @@ serial.c - Low level functions for sending and recieving bytes via the serial port Part of Grbl + Copyright (c) 2011-2014 Sungeun K. Jeon Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2011-2012 Sungeun K. 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 diff --git a/settings.c b/settings.c index 1ba0629..ae36abf 100644 --- a/settings.c +++ b/settings.c @@ -2,8 +2,8 @@ settings.c - eeprom configuration handling Part of Grbl + Copyright (c) 2011-2014 Sungeun K. Jeon Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2011-2013 Sungeun K. 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 diff --git a/settings.h b/settings.h index 7d435d9..04883b9 100644 --- a/settings.h +++ b/settings.h @@ -2,8 +2,8 @@ settings.h - eeprom configuration handling Part of Grbl + Copyright (c) 2011-2014 Sungeun K. Jeon Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2011-2013 Sungeun K. 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 diff --git a/spindle_control.c b/spindle_control.c index 5bfe621..5786762 100644 --- a/spindle_control.c +++ b/spindle_control.c @@ -2,8 +2,8 @@ spindle_control.c - spindle control methods Part of Grbl + Copyright (c) 2012-2014 Sungeun K. Jeon Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2012 Sungeun K. 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 diff --git a/spindle_control.h b/spindle_control.h index d3fca1e..ba8f051 100644 --- a/spindle_control.h +++ b/spindle_control.h @@ -2,8 +2,8 @@ spindle_control.h - spindle control methods Part of Grbl + Copyright (c) 2012-2014 Sungeun K. Jeon Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2012 Sungeun K. 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 diff --git a/stepper.c b/stepper.c index b9a80f9..128646c 100644 --- a/stepper.c +++ b/stepper.c @@ -2,7 +2,7 @@ stepper.c - stepper motor driver: executes motion plans using stepper motors Part of Grbl - Copyright (c) 2011-2013 Sungeun K. Jeon + Copyright (c) 2011-2014 Sungeun K. Jeon Copyright (c) 2009-2011 Simen Svale Skogsrud Grbl is free software: you can redistribute it and/or modify @@ -63,7 +63,11 @@ typedef struct { uint16_t n_step; // Number of step events to be executed for this segment uint8_t st_block_index; // Stepper block data index. Uses this information to execute this segment. uint16_t cycles_per_tick; // Step distance traveled per ISR tick, aka step rate. - uint8_t amass_level; + #ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING + uint8_t amass_level; + #else + uint8_t prescaler; + #endif } segment_t; static segment_t segment_buffer[SEGMENT_BUFFER_SIZE]; @@ -83,7 +87,9 @@ typedef struct { uint8_t step_pulse_time; // Step pulse reset time after step rise uint8_t step_outbits; // The next stepping-bits to be output uint8_t dir_outbits; - uint32_t steps[N_AXIS]; + #ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING + uint32_t steps[N_AXIS]; + #endif uint16_t step_count; // Steps remaining in line segment motion uint8_t exec_block_index; // Tracks the current st_block index. Change indicates new block. @@ -278,7 +284,9 @@ ISR(TIMER1_COMPA_vect) // Initialize new step segment and load number of steps to execute st.exec_segment = &segment_buffer[segment_buffer_tail]; // Initialize step segment timing per step and load number of steps to execute. - // TCCR1B = (TCCR1B & ~(0x07<prescaler<prescaler<cycles_per_tick; st.step_count = st.exec_segment->n_step; // NOTE: Can sometimes be zero when moving slow. // If the new segment starts a new planner block, initialize stepper variables and counters. @@ -292,9 +300,11 @@ ISR(TIMER1_COMPA_vect) st.counter_y = st.counter_x; st.counter_z = st.counter_x; } - st.steps[X_AXIS] = st.exec_block->steps[X_AXIS] >> st.exec_segment->amass_level; - st.steps[Y_AXIS] = st.exec_block->steps[Y_AXIS] >> st.exec_segment->amass_level; - st.steps[Z_AXIS] = st.exec_block->steps[Z_AXIS] >> st.exec_segment->amass_level; + #ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING + st.steps[X_AXIS] = st.exec_block->steps[X_AXIS] >> st.exec_segment->amass_level; + st.steps[Y_AXIS] = st.exec_block->steps[Y_AXIS] >> st.exec_segment->amass_level; + st.steps[Z_AXIS] = st.exec_block->steps[Z_AXIS] >> st.exec_segment->amass_level; + #endif } else { // Segment buffer empty. Shutdown. st_go_idle(); @@ -307,21 +317,33 @@ ISR(TIMER1_COMPA_vect) st.step_outbits = 0; // Execute step displacement profile by Bresenham line algorithm - st.counter_x += st.steps[X_AXIS]; + #ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING + st.counter_x += st.steps[X_AXIS]; + #else + st.counter_x += st.exec_block->steps[X_AXIS]; + #endif if (st.counter_x > st.exec_block->step_event_count) { st.step_outbits |= (1<step_event_count; if (st.exec_block->direction_bits & (1<steps[Y_AXIS]; + #endif if (st.counter_y > st.exec_block->step_event_count) { st.step_outbits |= (1<step_event_count; if (st.exec_block->direction_bits & (1<steps[Z_AXIS]; + #endif if (st.counter_z > st.exec_block->step_event_count) { st.step_outbits |= (1<step_event_count; @@ -433,6 +455,7 @@ void st_cycle_start() sys.state = STATE_CYCLE; st_prep_buffer(); // Initialize step segment buffer before beginning cycle. st_wake_up(); + if (bit_isfalse(settings.flags,BITFLAG_AUTO_START)) { sys.auto_start = false; } // Reset auto start per settings. } } @@ -500,9 +523,9 @@ void st_update_plan_block_parameters() */ void st_prep_buffer() { - if (sys.state == STATE_QUEUED) { return; } // Block until a motion state is issued while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer. - + if (sys.state == STATE_QUEUED) { return; } // Block until a motion state is issued + // Determine if we need to load a new planner block or if the block remainder is replanned. if (pl_block == NULL) { pl_block = plan_get_current_block(); // Query planner for a queued block @@ -520,16 +543,16 @@ void st_prep_buffer() // when the segment buffer completes the planner block, it may be discarded immediately. st_prep_block = &st_block_buffer[prep.st_block_index]; st_prep_block->direction_bits = pl_block->direction_bits; - #ifdef ACTIVE_MULTI_AXIS_STEP_SMOOTHING - st_prep_block->steps[X_AXIS] = pl_block->steps[X_AXIS] << MAX_AMASS_LEVEL; - st_prep_block->steps[Y_AXIS] = pl_block->steps[Y_AXIS] << MAX_AMASS_LEVEL; - st_prep_block->steps[Z_AXIS] = pl_block->steps[Z_AXIS] << MAX_AMASS_LEVEL; - st_prep_block->step_event_count = pl_block->step_event_count << MAX_AMASS_LEVEL; - #else + #ifndef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING st_prep_block->steps[X_AXIS] = pl_block->steps[X_AXIS]; st_prep_block->steps[Y_AXIS] = pl_block->steps[Y_AXIS]; st_prep_block->steps[Z_AXIS] = pl_block->steps[Z_AXIS]; st_prep_block->step_event_count = pl_block->step_event_count; + #else + st_prep_block->steps[X_AXIS] = pl_block->steps[X_AXIS] << MAX_AMASS_LEVEL; + st_prep_block->steps[Y_AXIS] = pl_block->steps[Y_AXIS] << MAX_AMASS_LEVEL; + st_prep_block->steps[Z_AXIS] = pl_block->steps[Z_AXIS] << MAX_AMASS_LEVEL; + st_prep_block->step_event_count = pl_block->step_event_count << MAX_AMASS_LEVEL; #endif // Initialize segment buffer data for generating the segments. @@ -701,8 +724,8 @@ void st_prep_buffer() float inv_rate = dt/(prep.steps_remaining-steps_remaining); cycles = ceil( (TICKS_PER_MICROSECOND*1000000*60)*inv_rate ); // (cycles/step) - #ifdef ACTIVE_MULTI_AXIS_STEP_SMOOTHING - // Compute step timing and multi-axis smoothing level. + #ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING + // Compute step timing and multi-axis smoothing level. // NOTE: Only one prescalar is required with AMASS enabled. if (cycles > AMASS_LEVEL1) { if (cycles > AMASS_LEVEL2) { @@ -712,8 +735,24 @@ void st_prep_buffer() cycles >>= prep_segment->amass_level; prep_segment->n_step <<= prep_segment->amass_level; } else { prep_segment->amass_level = 0; } - if (cycles < (1UL << 16)) { prep_segment->cycles_per_tick = cycles; } - else { prep_segment->cycles_per_tick = 0xffff; } // Just set the slowest speed possible. (4.1ms @ 16MHz) + if (cycles < (1UL << 16)) { prep_segment->cycles_per_tick = cycles; } // < 65536 (4.1ms @ 16MHz) + else { prep_segment->cycles_per_tick = 0xffff; } // Just set the slowest speed possible. + #else + // Compute step timing and timer prescalar for normal step generation. + if (cycles < (1UL << 16)) { // < 65536 (4.1ms @ 16MHz) + prep_segment->prescaler = 1; // prescaler: 0 + prep_segment->cycles_per_tick = cycles; + } else if (cycles < (1UL << 19)) { // < 524288 (32.8ms@16MHz) + prep_segment->prescaler = 2; // prescaler: 8 + prep_segment->cycles_per_tick = cycles >> 3; + } else { + prep_segment->prescaler = 3; // prescaler: 64 + if (cycles < (1UL << 22)) { // < 4194304 (262ms@16MHz) + prep_segment->cycles_per_tick = cycles >> 6; + } else { // Just set the slowest speed possible. + prep_segment->cycles_per_tick = 0xffff; + } + } #endif // Determine end of segment conditions. Setup initial conditions for next segment. @@ -754,8 +793,6 @@ void st_prep_buffer() // int32_t blength = segment_buffer_head - segment_buffer_tail; // if (blength < 0) { blength += SEGMENT_BUFFER_SIZE; } // printInteger(blength); - - if (sys.state & (STATE_QUEUED | STATE_HOMING)) { return; } // Force exit or one prepped segment. } } @@ -785,31 +822,3 @@ void st_prep_buffer() we know when the plan is feasible in the context of what's already in the code and not require too much more code? */ - -// static void st_config_step_timer(uint32_t cycles) -// { -// if (cycles < (1UL << 16)) { // < 65536 (4.1ms @ 16MHz) -// prep_segment->prescaler = 1; // prescaler: 0 -// prep_segment->cycles_per_tick = cycles; -// } else { -// prep_segment->prescaler = 2; // prescaler: 8 -// if (cycles < (1UL << 19)) { // < 524288 (32.8ms@16MHz) -// prep_segment->cycles_per_tick = cycles >> 3; -// -// // } else if (cycles < (1UL << 22)) { // < 4194304 (262ms@16MHz) -// // prep_segment->prescaler = 3; // prescaler: 64 -// // prep_segment->cycles_per_tick = cycles >> 6; -// // } else if (cycles < (1UL << 24)) { // < 16777216 (1.05sec@16MHz) -// // prep_segment->prescaler = 4; // prescaler: 256 -// // prep_segment->cycles_per_tick = (cycles >> 8); -// // } else { -// // prep_segment->prescaler = 5; // prescaler: 1024 -// // if (cycles < (1UL << 26)) { // < 67108864 (4.19sec@16MHz) -// // prep_segment->cycles_per_tick = (cycles >> 10); -// -// } else { // Just set the slowest speed possible. -// prep_segment->cycles_per_tick = 0xffff; -// } -// printString("X"); -// } -// } \ No newline at end of file diff --git a/stepper.h b/stepper.h index 9fc2c73..a72a565 100644 --- a/stepper.h +++ b/stepper.h @@ -2,8 +2,8 @@ stepper.h - stepper motor driver: executes motion plans of planner.c using the stepper motors Part of Grbl + Copyright (c) 2011-2014 Sungeun K. Jeon Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2011-2013 Sungeun K. 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 From c0381799eb9f4e809350c4f3e3c059c0dab849b1 Mon Sep 17 00:00:00 2001 From: Rob Brown Date: Thu, 2 Jan 2014 12:42:22 +0800 Subject: [PATCH 32/73] PWM Spindle Control and Invert Spindle & Coolant Pins PWM Spindle Control and Invert Spindle & Coolant Pins --- coolant_control.c | 25 +++++++-- cpu_map.h | 134 +++++++++++++++++++++++++++++++++++++++------- gcode.c | 5 +- gcode.h | 2 +- spindle_control.c | 76 +++++++++++++++++++------- spindle_control.h | 4 +- 6 files changed, 198 insertions(+), 48 deletions(-) diff --git a/coolant_control.c b/coolant_control.c index b866aef..90cde68 100644 --- a/coolant_control.c +++ b/coolant_control.c @@ -39,10 +39,17 @@ void coolant_init() void coolant_stop() { +#ifdef INVERT_COOLANT #ifdef ENABLE_M7 - COOLANT_MIST_PORT &= ~(1 << COOLANT_MIST_BIT); - #endif - COOLANT_FLOOD_PORT &= ~(1 << COOLANT_FLOOD_BIT); + COOLANT_MIST_PORT |= (1 << COOLANT_MIST_BIT); + #endif + COOLANT_FLOOD_PORT |= (1 << COOLANT_FLOOD_BIT); +#else +#ifdef ENABLE_M7 + COOLANT_MIST_PORT &= ~(1 << COOLANT_MIST_BIT); +#endif + COOLANT_FLOOD_PORT &= ~(1 << COOLANT_FLOOD_BIT); +#endif } @@ -51,11 +58,19 @@ void coolant_run(uint8_t mode) if (mode != current_coolant_mode) { plan_synchronize(); // Ensure coolant turns on when specified in program. - if (mode == COOLANT_FLOOD_ENABLE) { - COOLANT_FLOOD_PORT |= (1 << COOLANT_FLOOD_BIT); + if (mode == COOLANT_FLOOD_ENABLE) { +#ifdef INVERT_COOLANT + COOLANT_FLOOD_PORT &= ~(1 << COOLANT_FLOOD_BIT); +#else + COOLANT_FLOOD_PORT |= (1 << COOLANT_FLOOD_BIT); +#endif #ifdef ENABLE_M7 } else if (mode == COOLANT_MIST_ENABLE) { +#ifdef INVERT_COOLANT + COOLANT_MIST_PORT &= ~(1 << COOLANT_MIST_BIT); +#else COOLANT_MIST_PORT |= (1 << COOLANT_MIST_BIT); +#endif #endif } else { coolant_stop(); diff --git a/cpu_map.h b/cpu_map.h index ed97275..70a3984 100644 --- a/cpu_map.h +++ b/cpu_map.h @@ -34,6 +34,9 @@ #define SERIAL_RX USART_RX_vect #define SERIAL_UDRE USART_UDRE_vect + // Start of PWM & Stepper Enabled Spindle + // #define VARIABLE_SPINDLE // comment this out to disable PWM & Stepper on the spindle + // NOTE: All step bit and direction pins must be on the same port. #define STEPPING_DDR DDRD #define STEPPING_PORT PORTD @@ -54,6 +57,20 @@ #define STEPPERS_DISABLE_BIT 0 // Uno Digital Pin 8 #define STEPPERS_DISABLE_MASK (1< 0) { - SPINDLE_DIRECTION_PORT &= ~(1< 0) { + SPINDLE_DIRECTION_PORT &= ~(1< void spindle_init(); -void spindle_run(int8_t direction); //, uint16_t rpm); +void spindle_run(int8_t direction, uint16_t rpm); void spindle_stop(); +uint8_t spindle_pwm(); +void spindle_pwm_update(uint8_t pwm); #endif From f8dd8fa54d21c17b9aaddb5836b6a8d73d59640d Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Thu, 2 Jan 2014 12:12:35 -0700 Subject: [PATCH 33/73] Fix for M7/8/9 modal group checks. Updated AMASS frequency cutoffs and code cleaned. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Updated Grbl version to 0.9c and build number. - G-code parser was missing modal group violation checks for M7/8/9 commands. Added them. - Updated the Adaptive Multi-Axis Step Smoothing (AMASS) cutoff frequencies so that the trade between the 16-bit Timer1 accuracy and the level step smoothing are somewhat better balanced. (Smoothing isn’t free, but a higher accuracy timer would provide high cutoff frequencies.) --- gcode.c | 1 + gcode.h | 3 ++- settings.h | 4 ++-- stepper.c | 28 ++++++++++++++-------------- 4 files changed, 19 insertions(+), 17 deletions(-) diff --git a/gcode.c b/gcode.c index d8abaf6..eeaa433 100644 --- a/gcode.c +++ b/gcode.c @@ -179,6 +179,7 @@ uint8_t gc_execute_line(char *line) switch(int_value) { case 0: case 1: case 2: case 30: group_number = MODAL_GROUP_4; break; case 3: case 4: case 5: group_number = MODAL_GROUP_7; break; + case 7: case 8: case 9: group_number = MODAL_GROUP_8; break; } // Set 'M' commands switch(int_value) { diff --git a/gcode.h b/gcode.h index 8c20957..334c78a 100644 --- a/gcode.h +++ b/gcode.h @@ -38,7 +38,8 @@ #define MODAL_GROUP_5 6 // [G93,G94] Feed rate mode #define MODAL_GROUP_6 7 // [G20,G21] Units #define MODAL_GROUP_7 8 // [M3,M4,M5] Spindle turning -#define MODAL_GROUP_12 9 // [G54,G55,G56,G57,G58,G59] Coordinate system selection +#define MODAL_GROUP_8 9 // [M7,M8,M9] Coolant control +#define MODAL_GROUP_12 10 // [G54,G55,G56,G57,G58,G59] Coordinate system selection // Define command actions for within execution-type modal groups (motion, stopping, non-modal). Used // internally by the parser to know which command to execute. diff --git a/settings.h b/settings.h index 04883b9..40c5cca 100644 --- a/settings.h +++ b/settings.h @@ -25,8 +25,8 @@ #include #include "nuts_bolts.h" -#define GRBL_VERSION "0.9b" -#define GRBL_VERSION_BUILD "20131210" +#define GRBL_VERSION "0.9c" +#define GRBL_VERSION_BUILD "20131231" // Version of the EEPROM data. Will be used to migrate existing data from older versions of Grbl // when firmware is upgraded. Always stored in byte 0 of eeprom diff --git a/stepper.c b/stepper.c index 128646c..a012677 100644 --- a/stepper.c +++ b/stepper.c @@ -34,11 +34,13 @@ #define RAMP_CRUISE 1 #define RAMP_DECEL 2 - +// Can't have a high of a cutoff frequency. The 16-bit timer isn't as accurate as it seems. +// There is a trade between the accuracy of the timer and the smoothness of multi-axis steps. +// #define MAX_AMASS_LEVEL 3 -#define AMASS_LEVEL1 (F_CPU/10000) -#define AMASS_LEVEL2 (F_CPU/5000) -#define AMASS_LEVEL3 (F_CPU/2500) +#define AMASS_LEVEL1 (F_CPU/8000) +#define AMASS_LEVEL2 (F_CPU/4000) +#define AMASS_LEVEL3 (F_CPU/2000) // Stores the planner block Bresenham algorithm execution data for the segments in the segment @@ -134,8 +136,6 @@ typedef struct { static st_prep_t prep; -static void st_config_step_timer(uint32_t cycles); - /* BLOCK VELOCITY PROFILE DEFINITION __________________________ /| |\ _________________ ^ @@ -727,14 +727,14 @@ void st_prep_buffer() #ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING // Compute step timing and multi-axis smoothing level. // NOTE: Only one prescalar is required with AMASS enabled. - if (cycles > AMASS_LEVEL1) { - if (cycles > AMASS_LEVEL2) { - if (cycles > AMASS_LEVEL3) { prep_segment->amass_level = 3; } - else { prep_segment->amass_level = 2; } - } else { prep_segment->amass_level = 1; } + if (cycles < AMASS_LEVEL1) { prep_segment->amass_level = 0; } + else { + if (cycles < AMASS_LEVEL2) { prep_segment->amass_level = 1; } + else if (cycles < AMASS_LEVEL3) { prep_segment->amass_level = 2; } + else { prep_segment->amass_level = 3; } cycles >>= prep_segment->amass_level; prep_segment->n_step <<= prep_segment->amass_level; - } else { prep_segment->amass_level = 0; } + } if (cycles < (1UL << 16)) { prep_segment->cycles_per_tick = cycles; } // < 65536 (4.1ms @ 16MHz) else { prep_segment->cycles_per_tick = 0xffff; } // Just set the slowest speed possible. #else @@ -749,7 +749,7 @@ void st_prep_buffer() prep_segment->prescaler = 3; // prescaler: 64 if (cycles < (1UL << 22)) { // < 4194304 (262ms@16MHz) prep_segment->cycles_per_tick = cycles >> 6; - } else { // Just set the slowest speed possible. + } else { // Just set the slowest speed possible. (Around 4 step/sec.) prep_segment->cycles_per_tick = 0xffff; } } @@ -778,7 +778,7 @@ void st_prep_buffer() if (sys.state == STATE_HOLD) { if (prep.current_speed == 0.0) { - // TODO: Check if the segment buffer gets initialized correctly. +// TODO: Check if the segment buffer gets initialized correctly. plan_cycle_reinitialize(); sys.state = STATE_QUEUED; } From ce5f94d6759406987297125b892be447bd388919 Mon Sep 17 00:00:00 2001 From: Rob Brown Date: Fri, 3 Jan 2014 07:42:22 +0800 Subject: [PATCH 34/73] Update spindle_control.c Updated spindle_control.c due to compile error. --- spindle_control.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/spindle_control.c b/spindle_control.c index 8fb51e0..5dfab0b 100644 --- a/spindle_control.c +++ b/spindle_control.c @@ -93,5 +93,5 @@ uint8_t spindle_pwm() void spindle_pwm_update(uint8_t pwm) { - OCRA_REGISTER = pwm; + OCR_REGISTER = pwm; } From 1c4097352dd089d6c445db491e76cc5dc9d4b6f7 Mon Sep 17 00:00:00 2001 From: Rob Brown Date: Fri, 3 Jan 2014 19:35:56 +0800 Subject: [PATCH 35/73] Update to fix compile error Update to fix compile error --- spindle_control.c | 10 +++++++++- spindle_control.h | 3 +++ 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/spindle_control.c b/spindle_control.c index 5dfab0b..813ee8a 100644 --- a/spindle_control.c +++ b/spindle_control.c @@ -25,7 +25,10 @@ static uint8_t current_direction; static uint16_t current_rpm; -static uint8_t current_pwm; + +#ifdef VARIABLE_SPINDLE +static uint8_t current_pwm; // Stores the PWM set by the S value +#endif void spindle_init() { @@ -86,12 +89,17 @@ void spindle_run(int8_t direction, uint16_t rpm) { } } +#ifdef VARIABLE_SPINDLE uint8_t spindle_pwm() { + // This function can be used by the st_prep_buffer() to calculate what the spindle speed vs travel speed return current_pwm; } void spindle_pwm_update(uint8_t pwm) { + // This function can be used by the stepper interrupt to set the spindle speed OCR_REGISTER = pwm; + } +#endif \ No newline at end of file diff --git a/spindle_control.h b/spindle_control.h index 4af96f1..9e4384f 100644 --- a/spindle_control.h +++ b/spindle_control.h @@ -27,7 +27,10 @@ void spindle_init(); void spindle_run(int8_t direction, uint16_t rpm); void spindle_stop(); + +#ifdef VARIABLE_SPINDLE uint8_t spindle_pwm(); void spindle_pwm_update(uint8_t pwm); +#endif #endif From 8b5f30685108fa6b0e196f50c26a11670f68a1a5 Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Sat, 4 Jan 2014 12:12:44 -0700 Subject: [PATCH 36/73] Cleaned up variable spindle output (PWM). Code and config comments. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Variable spindle speed output as a configuration option. Thanks @EliteEng! When enabled, the Z-limit (D11) and spindle enable(D12) pins switch to allow access to the hardware PWM output on pin D11. Otherwise, everything should work as it does. - Removed option for inverting the spindle and coolant enable pins. This is a safety hazard, especially for the spindle. When Grbl initializes, all pins are momentarily low until it finishes booting. If an invert is enabled, this means the spindles can be energized briefly during this time. If users need signal inversion, it’s recommended to just wire in an inversion circuit instead. - Cleared out references to spindle variable output in terms of step signal. This isn’t complete and requires more deliberation before installing. - Cleared up and cleaned up some code and config comments. --- config.h | 26 +++++- coolant_control.c | 39 +++------ cpu_map.h | 217 +++++++++++++++++++--------------------------- limits.c | 2 +- spindle_control.c | 98 ++++++++++----------- spindle_control.h | 7 +- 6 files changed, 183 insertions(+), 206 deletions(-) diff --git a/config.h b/config.h index daa6268..4cb4de5 100644 --- a/config.h +++ b/config.h @@ -86,10 +86,24 @@ // acceleration, particularly noticeable on machines that run at very high feedrates, but may negatively // impact performance. 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. +// NOTE: Changing this value also changes the execution time of a segment in the step segment buffer. +// When increasing this value, this stores less overall time in the segment buffer and vice versa. Make +// certain the step segment buffer is increased/decreased to account for these changes. #define ACCELERATION_TICKS_PER_SECOND 100 -#define ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING -#define ENABLE_SOFTWARE_DEBOUNCE +// Adaptive Multi-Axis Step Smoothing (AMASS) is an advanced feature that does what its name implies, +// smoothing the stepping of multi-axis motions. This feature smooths motion particularly at low step +// frequencies below 10kHz, where the aliasing between axes of multi-axis motions can cause audible +// noise and shake your machine. At even lower step frequencies, AMASS adapts and provides even better +// step smoothing. See stepper.c for more details on the AMASS system works. +#define ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING // Default enabled. Comment to disable. + +// Enables variable spindle output voltage for different RPM values. On the Arduino Uno, the spindle +// enable pin will output 5V for maximum RPM with 256 intermediate levels and 0V when disabled. +// NOTE: IMPORTANT for Arduino Unos! When enabled, the Z-limit pin D11 and spindle enable pin D12 switch! +// The hardware PWM output on pin D11 is required for variable spindle output voltages. +// #define VARIABLE_SPINDLE // Default disabled. Uncomment to enable. +// #define SPINDLE_MAX_RPM 1000 // Max spindle RPM. This value is equal to 100% Duty Cycle on the PWM. // Minimum planner junction speed. Sets the default minimum junction speed the planner plans to at // every buffer block junction, except for starting from rest and end of the buffer, which are always @@ -168,6 +182,14 @@ // case, please report any successes to grbl administrators! // #define ENABLE_XONXOFF // Default disabled. Uncomment to enable. +// A simple software debouncing feature for hard limit switches. When enabled, the interrupt monitoring +// the hard limit switch pins will enable the Arduino's watchdog timer to re-check the limit pin state +// after a delay of about 32msec. This can help with CNC machines with problematic false triggering of +// their hard limit switches, but it WILL NOT fix issues with electrical interference on the signal +// cables from external sources. It's recommended to first use shielded signal cables that are grounded +// (old USB/computer cables work well) and wire in a low-pass circuit into each limit pin. +// #define ENABLE_SOFTWARE_DEBOUNCE // Default disabled. Uncomment to enable. + // --------------------------------------------------------------------------------------- // TODO: Install compile-time option to send numeric status codes rather than strings. diff --git a/coolant_control.c b/coolant_control.c index 90cde68..b849a78 100644 --- a/coolant_control.c +++ b/coolant_control.c @@ -27,29 +27,24 @@ static uint8_t current_coolant_mode; + void coolant_init() { current_coolant_mode = COOLANT_DISABLE; - #if ENABLE_M7 + COOLANT_FLOOD_DDR |= (1 << COOLANT_FLOOD_BIT); + #ifdef ENABLE_M7 COOLANT_MIST_DDR |= (1 << COOLANT_MIST_BIT); #endif - COOLANT_FLOOD_DDR |= (1 << COOLANT_FLOOD_BIT); coolant_stop(); } + void coolant_stop() { -#ifdef INVERT_COOLANT + COOLANT_FLOOD_PORT &= ~(1 << COOLANT_FLOOD_BIT); #ifdef ENABLE_M7 - COOLANT_MIST_PORT |= (1 << COOLANT_MIST_BIT); - #endif - COOLANT_FLOOD_PORT |= (1 << COOLANT_FLOOD_BIT); -#else -#ifdef ENABLE_M7 - COOLANT_MIST_PORT &= ~(1 << COOLANT_MIST_BIT); -#endif - COOLANT_FLOOD_PORT &= ~(1 << COOLANT_FLOOD_BIT); -#endif + COOLANT_MIST_PORT &= ~(1 << COOLANT_MIST_BIT); + #endif } @@ -59,19 +54,13 @@ void coolant_run(uint8_t mode) { plan_synchronize(); // Ensure coolant turns on when specified in program. if (mode == COOLANT_FLOOD_ENABLE) { -#ifdef INVERT_COOLANT - COOLANT_FLOOD_PORT &= ~(1 << COOLANT_FLOOD_BIT); -#else - COOLANT_FLOOD_PORT |= (1 << COOLANT_FLOOD_BIT); -#endif - #ifdef ENABLE_M7 - } else if (mode == COOLANT_MIST_ENABLE) { -#ifdef INVERT_COOLANT - COOLANT_MIST_PORT &= ~(1 << COOLANT_MIST_BIT); -#else - COOLANT_MIST_PORT |= (1 << COOLANT_MIST_BIT); -#endif - #endif + COOLANT_FLOOD_PORT |= (1 << COOLANT_FLOOD_BIT); + + #ifdef ENABLE_M7 + } else if (mode == COOLANT_MIST_ENABLE) { + COOLANT_MIST_PORT |= (1 << COOLANT_MIST_BIT); + #endif + } else { coolant_stop(); } diff --git a/cpu_map.h b/cpu_map.h index 70a3984..bbc006b 100644 --- a/cpu_map.h +++ b/cpu_map.h @@ -28,94 +28,82 @@ #ifndef cpu_map_h #define cpu_map_h +//---------------------------------------------------------------------------------------- + #ifdef CPU_MAP_ATMEGA328P // (Arduino Uno) Officially supported by Grbl. - // Serial port pins + // Define serial port pins and interrupt vectors. #define SERIAL_RX USART_RX_vect #define SERIAL_UDRE USART_UDRE_vect // Start of PWM & Stepper Enabled Spindle // #define VARIABLE_SPINDLE // comment this out to disable PWM & Stepper on the spindle - // NOTE: All step bit and direction pins must be on the same port. - #define STEPPING_DDR DDRD - #define STEPPING_PORT PORTD - #define X_STEP_BIT 2 // Uno Digital Pin 2 - #define Y_STEP_BIT 3 // Uno Digital Pin 3 - #define Z_STEP_BIT 4 // Uno Digital Pin 4 - #define STEP_MASK ((1< 0) { - SPINDLE_DIRECTION_PORT &= ~(1< 0) { + SPINDLE_DIRECTION_PORT &= ~(1< SPINDLE_MAX_RPM) { rpm = SPINDLE_MAX_RPM; } // Prevent overflow. + uint8_t current_pwm = floor((((float) rpm / (float) SPINDLE_MAX_RPM ) * 255.0) + 0.5); + OCR_REGISTER = current_pwm; + + #ifndef CPU_MAP_ATMEGA328P // On the Uno, spindle enable and PWM are shared. + SPINDLE_ENABLE_PORT |= (1< +// Initializes spindle pins and hardware PWM, if enabled. void spindle_init(); + +// Sets spindle direction and spindle rpm via PWM, if enabled. void spindle_run(int8_t direction, uint16_t rpm); + +// Kills spindle. void spindle_stop(); -uint8_t spindle_pwm(); -void spindle_pwm_update(uint8_t pwm); #endif From 3c3382ff7557a8dff61505b0020f50fdb77d1190 Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Sun, 5 Jan 2014 10:27:34 -0700 Subject: [PATCH 37/73] New build info feature. (per @Analogreality request) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - New build info feature. Hidden command ‘$I’ will print the build info for your Grbl firmware. Users may also write an identifying message within it via ‘$I=‘ with up to 32 characters. (no more, or else it will break). - Adjusted the max number of startup lines to 3. Majority of people will only need one. - Fixed a compile error with spindle_control.c. A rogue #endif was causing problems. --- config.h | 4 ++-- protocol.c | 16 ++++++++++++++++ report.c | 12 +++++++++++- report.h | 3 +++ settings.c | 19 +++++++++++++++++++ settings.h | 5 +++++ spindle_control.c | 2 -- 7 files changed, 56 insertions(+), 5 deletions(-) diff --git a/config.h b/config.h index 4cb4de5..78d0ecf 100644 --- a/config.h +++ b/config.h @@ -74,10 +74,10 @@ #define N_HOMING_LOCATE_CYCLE 2 // Integer (1-128) // Number of blocks Grbl executes upon startup. These blocks are stored in EEPROM, where the size -// and addresses are defined in settings.h. With the current settings, up to 5 startup blocks may +// and addresses are defined in settings.h. With the current settings, up to 3 startup blocks may // be stored and executed in order. These startup blocks would typically be used to set the g-code // parser state depending on user preferences. -#define N_STARTUP_LINE 2 // Integer (1-5) +#define N_STARTUP_LINE 2 // Integer (1-3) // --------------------------------------------------------------------------------------- // ADVANCED CONFIGURATION OPTIONS: diff --git a/protocol.c b/protocol.c index 29d81e6..0cc0319 100644 --- a/protocol.c +++ b/protocol.c @@ -254,6 +254,22 @@ uint8_t protocol_execute_line(char *line) if (!sys.abort) { protocol_execute_startup(); } // Execute startup scripts after successful homing. } else { return(STATUS_SETTING_DISABLED); } break; + case 'I' : // Print or store build info. + if ( line[++char_counter] == 0 ) { + if (!(settings_read_build_info(line))) { + report_status_message(STATUS_SETTING_READ_FAIL); + } else { + report_build_info(line); + } + } else { // Store startup line + if(line[char_counter++] != '=') { return(STATUS_UNSUPPORTED_STATEMENT); } + helper_var = char_counter; // Set helper variable as counter to start of user info line. + do { + line[char_counter-helper_var] = line[char_counter]; + } while (line[char_counter++] != 0); + settings_store_build_info(line); + } + break; case 'N' : // Startup lines. if ( line[++char_counter] == 0 ) { // Print startup lines for (helper_var=0; helper_var < N_STARTUP_LINE; helper_var++) { diff --git a/report.c b/report.c index 7e32ef1..1796c8c 100644 --- a/report.c +++ b/report.c @@ -125,7 +125,7 @@ void report_feedback_message(uint8_t message_code) // Welcome message void report_init_message() { - printPgmString(PSTR("\r\nGrbl " GRBL_VERSION " ("GRBL_VERSION_BUILD ") ['$' for help]\r\n")); + printPgmString(PSTR("\r\nGrbl " GRBL_VERSION " ['$' for help]\r\n")); } // Grbl help message @@ -292,6 +292,16 @@ void report_startup_line(uint8_t n, char *line) printPgmString(PSTR("\r\n")); } + +// Prints build info line +void report_build_info(char *line) +{ + printPgmString(PSTR("[" GRBL_VERSION "." GRBL_VERSION_BUILD ":")); + printString(line); + printPgmString(PSTR("]\r\n")); +} + + // Prints real-time data. This function grabs a real-time snapshot of the stepper subprogram // and the actual location of the CNC machine. Users may change the following function to their // specific needs, but the desired real-time data report must be as short as possible. This is diff --git a/report.h b/report.h index 53c97dc..92984df 100644 --- a/report.h +++ b/report.h @@ -78,4 +78,7 @@ void report_gcode_modes(); // Prints startup line void report_startup_line(uint8_t n, char *line); +// Prints build info and user info +void report_build_info(char *line); + #endif diff --git a/settings.c b/settings.c index ae36abf..a312ee4 100644 --- a/settings.c +++ b/settings.c @@ -50,6 +50,12 @@ void settings_store_startup_line(uint8_t n, char *line) memcpy_to_eeprom_with_checksum(addr,(char*)line, LINE_BUFFER_SIZE); } +// Method to store build info into EEPROM +void settings_store_build_info(char *line) +{ + memcpy_to_eeprom_with_checksum(EEPROM_ADDR_BUILD_INFO,(char*)line, LINE_BUFFER_SIZE); +} + // Method to store coord data parameters into EEPROM void settings_write_coord_data(uint8_t coord_select, float *coord_data) { @@ -120,6 +126,19 @@ uint8_t settings_read_startup_line(uint8_t n, char *line) } } +// Reads startup line from EEPROM. Updated pointed line string data. +uint8_t settings_read_build_info(char *line) +{ + if (!(memcpy_from_eeprom_with_checksum((char*)line, EEPROM_ADDR_BUILD_INFO, LINE_BUFFER_SIZE))) { + // Reset line with default value + line[0] = 0; + settings_store_build_info(line); + return(false); + } else { + return(true); + } +} + // Read selected coordinate data from EEPROM. Updates pointed coord_data value. uint8_t settings_read_coord_data(uint8_t coord_select, float *coord_data) { diff --git a/settings.h b/settings.h index 40c5cca..05840bb 100644 --- a/settings.h +++ b/settings.h @@ -48,6 +48,7 @@ #define EEPROM_ADDR_GLOBAL 1 #define EEPROM_ADDR_PARAMETERS 512 #define EEPROM_ADDR_STARTUP_BLOCK 768 +#define EEPROM_ADDR_BUILD_INFO 992 // Define EEPROM address indexing for coordinate parameters #define N_COORDINATE_SYSTEM 6 // Number of supported work coordinate systems (from index 1) @@ -93,6 +94,10 @@ void settings_store_startup_line(uint8_t n, char *line); // Reads an EEPROM startup line to the protocol line variable uint8_t settings_read_startup_line(uint8_t n, char *line); +void settings_store_build_info(char *line); + +uint8_t settings_read_build_info(char *line); + // Writes selected coordinate data to EEPROM void settings_write_coord_data(uint8_t coord_select, float *coord_data); diff --git a/spindle_control.c b/spindle_control.c index 9dcc870..70e819e 100644 --- a/spindle_control.c +++ b/spindle_control.c @@ -92,5 +92,3 @@ void spindle_run(int8_t direction, uint16_t rpm) current_rpm = rpm; } } - -#endif From 7a85ab896d1640138f9612b6635299075e6f64d2 Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Sun, 5 Jan 2014 11:54:59 -0700 Subject: [PATCH 38/73] Updates to some stepper algorithm commenting --- protocol.c | 2 +- stepper.c | 63 +++++++++++++++++++++++++++++++++++------------------- 2 files changed, 42 insertions(+), 23 deletions(-) diff --git a/protocol.c b/protocol.c index 0cc0319..54c959f 100644 --- a/protocol.c +++ b/protocol.c @@ -249,7 +249,7 @@ uint8_t protocol_execute_line(char *line) break; case 'H' : // Perform homing cycle if (bit_istrue(settings.flags,BITFLAG_HOMING_ENABLE)) { - // Only perform homing if Grbl is idle or lost. + // Only perform homing if Grbl is idle or alarm. mc_homing_cycle(); if (!sys.abort) { protocol_execute_startup(); } // Execute startup scripts after successful homing. } else { return(STATUS_SETTING_DISABLED); } diff --git a/stepper.c b/stepper.c index a012677..7792896 100644 --- a/stepper.c +++ b/stepper.c @@ -28,63 +28,66 @@ // Some useful constants. -#define DT_SEGMENT (1.0/(ACCELERATION_TICKS_PER_SECOND*60.0)) // min/segment - +#define DT_SEGMENT (1.0/(ACCELERATION_TICKS_PER_SECOND*60.0)) // min/segment #define RAMP_ACCEL 0 #define RAMP_CRUISE 1 #define RAMP_DECEL 2 -// Can't have a high of a cutoff frequency. The 16-bit timer isn't as accurate as it seems. -// There is a trade between the accuracy of the timer and the smoothness of multi-axis steps. -// +// Define AMASS levels and cutoff frequencies. The highest level frequency bin starts at 0Hz and +// ends at its cutoff frequency. The next lower level frequency bin starts at the next higher cutoff +// frequency, and so on. The cutoff frequencies for each level must be considered carefully against +// how much it over-drives the stepper ISR, the accuracy of the 16-bit timer, and the CPU overhead. +// Level 0 (no AMASS, normal operation) frequency bin starts at the Level 1 cutoff frequency and +// up to as fast as the CPU allows (over 30kHz in limited testing). +// NOTE: AMASS uutoff frequency multiplied by ISR overdrive factor must not exceed maximum step frequency. +// NOTE: Current settings are set to overdrive the ISR to no more than 16kHz, balancing CPU overhead +// and timer accuracy. Do not alter these settings unless you know what you are doing. #define MAX_AMASS_LEVEL 3 -#define AMASS_LEVEL1 (F_CPU/8000) -#define AMASS_LEVEL2 (F_CPU/4000) -#define AMASS_LEVEL3 (F_CPU/2000) +// AMASS_LEVEL0: Normal operation. No AMASS. No upper cutoff frequency. +#define AMASS_LEVEL1 (F_CPU/8000) // Over-drives ISR (x2). Defined as F_CPU/(Cutoff frequency in Hz) +#define AMASS_LEVEL2 (F_CPU/4000) // Over-drives ISR (x4) +#define AMASS_LEVEL3 (F_CPU/2000) // Over-drives ISR (x8) // Stores the planner block Bresenham algorithm execution data for the segments in the segment // buffer. Normally, this buffer is partially in-use, but, for the worst case scenario, it will // never exceed the number of accessible stepper buffer segments (SEGMENT_BUFFER_SIZE-1). // NOTE: This data is copied from the prepped planner blocks so that the planner blocks may be -// discarded when entirely consumed and completed by the segment buffer. +// discarded when entirely consumed and completed by the segment buffer. Also, AMASS alters this +// data for its own use. typedef struct { uint8_t direction_bits; uint32_t steps[N_AXIS]; uint32_t step_event_count; } st_block_t; static st_block_t st_block_buffer[SEGMENT_BUFFER_SIZE-1]; -// TODO: Directly adjust this parameters to stop motion of individual axes for the homing cycle. -// But this may require this to be volatile if it is controlled by an interrupt. // Primary stepper segment ring buffer. Contains small, short line segments for the stepper // algorithm to execute, which are "checked-out" incrementally from the first block in the // planner buffer. Once "checked-out", the steps in the segments buffer cannot be modified by // the planner, where the remaining planner block steps still can. typedef struct { - uint16_t n_step; // Number of step events to be executed for this segment - uint8_t st_block_index; // Stepper block data index. Uses this information to execute this segment. - uint16_t cycles_per_tick; // Step distance traveled per ISR tick, aka step rate. + uint16_t n_step; // Number of step events to be executed for this segment + uint8_t st_block_index; // Stepper block data index. Uses this information to execute this segment. + uint16_t cycles_per_tick; // Step distance traveled per ISR tick, aka step rate. #ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING - uint8_t amass_level; + uint8_t amass_level; // Indicates AMASS level for the ISR to execute this segment #else - uint8_t prescaler; + uint8_t prescaler; // Without AMASS, a prescaler is required to adjust for slow timing. #endif } segment_t; static segment_t segment_buffer[SEGMENT_BUFFER_SIZE]; -// Stepper state variable. Contains running data and trapezoid variables. +// Stepper ISR data struct. Contains the running data for the main stepper ISR. typedef struct { // Used by the bresenham line algorithm uint32_t counter_x, // Counter variables for the bresenham line tracer counter_y, counter_z; - #ifdef STEP_PULSE_DELAY uint8_t step_bits; // Stores out_bits output to complete the step pulse delay #endif - // Used by the stepper driver interrupt uint8_t execute_step; // Flags step execution for each interrupt. uint8_t step_pulse_time; // Step pulse reset time after step rise uint8_t step_outbits; // The next stepping-bits to be output @@ -235,9 +238,25 @@ void st_go_idle() round-off errors and only requires fast integer counters, meaning low computational overhead and maximizing the Arduino's capabilities. However, the downside of the Bresenham algorithm is, for certain multi-axis motions, the non-dominant axes may suffer from un-smooth step - pulse trains, which can lead to strange audible noises or shaking. This is particularly - noticeable or may cause motion issues at low step frequencies (<1kHz), but usually not at - higher frequencies. + pulse trains, or aliasing, which can lead to strange audible noises or shaking. This is + particularly noticeable or may cause motion issues at low step frequencies (0-5kHz), but + is usually not a physical problem at higher frequencies, although audible. + To improve Bresenham multi-axis performance, Grbl uses what we call an Adaptive Multi-Axis + Step Smoothing (AMASS) algorithm, which does what the name implies. At lower step frequencies, + AMASS artificially increases the Bresenham resolution without effecting the algorithm's + innate exactness. AMASS adapts its resolution levels automatically depending on the step + frequency to be executed, meaning that for even lower step frequencies the step smoothing + level increases. Algorithmically, AMASS is acheived by a simple bit-shifting of the Bresenham + step count for each AMASS level. For example, for a Level 1 step smoothing, we bit shift + the Bresenham step event count, effectively multiplying it by 2, while the axis step counts + remain the same, and then double the stepper ISR frequency. In effect, we are allowing the + non-dominant Bresenham axes step in the intermediate ISR tick, while the dominant axis is + stepping every two ISR ticks, rather than every ISR tick in the traditional sense. At AMASS + Level 2, we simply bit-shift again, so the non-dominant Bresenham axes can step within any + of the four ISR ticks, and the dominant axis steps every four ISR ticks. And so on. This, + in effect, removes the vast majority of the multi-axis aliasing issues with the Bresenham + algorithm and does not significantly alter Grbl's performance, but in fact, more efficiently + utilizes unused CPU cycles overall throughout all configurations. This interrupt is simple and dumb by design. All the computational heavy-lifting, as in determining accelerations, is performed elsewhere. This interrupt pops pre-computed segments, defined as constant velocity over n number of steps, from the step segment buffer and then From cc9afdc195da0b2e71eda71ea24ddbd14378ceb1 Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Fri, 10 Jan 2014 20:22:10 -0700 Subject: [PATCH 39/73] Lots of re-organization and cleaning-up. Some bug fixes. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Added a new source and header file called system. These files contain the system commands and variables, as well as all of the system headers and standard libraries Grbl uses. Centralizing some of the code. - Re-organized the include headers throughout the source code. - ENABLE_M7 define was missing from config.h. Now there. - SPINDLE_MAX_RPM and SPINDLE_MIN_RPM now defined in config.h. No uncommenting to prevent user issues. Minimum spindle RPM now provides the lower, near 0V, scale adjustment, i.e. some spindles can go really slow so why use up our 256 voltage bins for them? - Remove some persistent variables from coolant and spindle control. They were redundant. - Removed a VARIABLE_SPINDLE define in cpu_map.h that shouldn’t have been there. - Changed the DEFAULT_ARC_TOLERANCE to 0.002mm to improve arc tracing. Before we had issues with performance, no longer. - Fixed a bug with the hard limits and the software debounce feature enabled. The invert limit pin setting wasn’t honored. - Fixed a bug with the homing direction mask. Now is like it used to be. At least for now. - Re-organized main.c to serve as only as the reset/initialization routine. Makes things a little bit clearer in terms of execution procedures. - Re-organized protocol.c as the overall master control unit for execution procedures. Not quite there yet, but starting to make a little more sense in how things are run. - Removed updating of old settings records. So many new settings have been added that it’s not worth adding the code to migrate old user settings. - Tweaked spindle_control.c a bit and made it more clear and consistent with other parts of Grbl. - Tweaked the stepper disable bit code in stepper.c. Requires less flash memory. --- Makefile | 2 +- config.h | 27 +++- coolant_control.c | 24 +--- coolant_control.h | 2 +- cpu_map.h | 3 - defaults.h | 12 +- gcode.c | 17 +-- gcode.h | 10 +- limits.c | 42 +++--- limits.h | 1 + main.c | 89 +++++-------- motion_control.c | 20 ++- motion_control.h | 2 - nuts_bolts.c | 6 +- nuts_bolts.h | 45 ------- planner.c | 8 +- planner.h | 3 +- print.c | 5 +- protocol.c | 330 ++++++++++++++-------------------------------- protocol.h | 21 +-- report.c | 9 +- serial.c | 10 +- serial.h | 1 - settings.c | 84 +++++------- settings.h | 8 +- spindle_control.c | 64 ++++----- spindle_control.h | 8 +- stepper.c | 67 +++++----- stepper.h | 4 +- system.c | 200 ++++++++++++++++++++++++++++ system.h | 95 +++++++++++++ 31 files changed, 636 insertions(+), 583 deletions(-) create mode 100644 system.c create mode 100644 system.h diff --git a/Makefile b/Makefile index 8ff9359..ea1ea61 100644 --- a/Makefile +++ b/Makefile @@ -33,7 +33,7 @@ CLOCK = 16000000 PROGRAMMER ?= -c avrisp2 -P usb OBJECTS = main.o motion_control.o gcode.o spindle_control.o coolant_control.o serial.o \ protocol.o stepper.o eeprom.o settings.o planner.o nuts_bolts.o limits.o \ - print.o report.o + print.o report.o system.o # FUSES = -U hfuse:w:0xd9:m -U lfuse:w:0x24:m FUSES = -U hfuse:w:0xd2:m -U lfuse:w:0xff:m # update that line with this when programmer is back up: diff --git a/config.h b/config.h index 78d0ecf..dbf37e3 100644 --- a/config.h +++ b/config.h @@ -79,6 +79,11 @@ // parser state depending on user preferences. #define N_STARTUP_LINE 2 // Integer (1-3) +// Enables a second coolant control pin via the mist coolant g-code command M7 on the Arduino Uno +// analog pin 5. Only use this option if you require a second control pin. +// NOTE: The M8 flood coolant control pin on analog pin 4 will still be functional regardless. +// #define ENABLE_M7 // Mist coolant disabled by default. See config.h to enable/disable. + // --------------------------------------------------------------------------------------- // ADVANCED CONFIGURATION OPTIONS: @@ -103,7 +108,14 @@ // NOTE: IMPORTANT for Arduino Unos! When enabled, the Z-limit pin D11 and spindle enable pin D12 switch! // The hardware PWM output on pin D11 is required for variable spindle output voltages. // #define VARIABLE_SPINDLE // Default disabled. Uncomment to enable. -// #define SPINDLE_MAX_RPM 1000 // Max spindle RPM. This value is equal to 100% Duty Cycle on the PWM. + +// Use by the variable spindle output only. These parameters set the maximum and minimum spindle speed +// "S" g-code values to correspond to the maximum and minimum pin voltages. There are 256 discrete and +// equally divided voltage bins between the maximum and minimum spindle speeds. So for a 5V pin, 1000 +// max rpm, and 250 min rpm, the spindle output voltage would be set for the following "S" commands: +// "S1000" @ 5V, "S250" @ 0.02V, and "S625" @ 2.5V (mid-range). The pin outputs 0V when disabled. +#define SPINDLE_MAX_RPM 1000.0 // Max spindle RPM. This value is equal to 100% duty cycle on the PWM. +#define SPINDLE_MIN_RPM 0.0 // Min spindle RPM. This value is equal to (1/256) duty cycle on the PWM. // Minimum planner junction speed. Sets the default minimum junction speed the planner plans to at // every buffer block junction, except for starting from rest and end of the buffer, which are always @@ -182,12 +194,13 @@ // case, please report any successes to grbl administrators! // #define ENABLE_XONXOFF // Default disabled. Uncomment to enable. -// A simple software debouncing feature for hard limit switches. When enabled, the interrupt monitoring -// the hard limit switch pins will enable the Arduino's watchdog timer to re-check the limit pin state -// after a delay of about 32msec. This can help with CNC machines with problematic false triggering of -// their hard limit switches, but it WILL NOT fix issues with electrical interference on the signal -// cables from external sources. It's recommended to first use shielded signal cables that are grounded -// (old USB/computer cables work well) and wire in a low-pass circuit into each limit pin. +// A simple software debouncing feature for hard limit switches. When enabled, the interrupt +// monitoring the hard limit switch pins will enable the Arduino's watchdog timer to re-check +// the limit pin state after a delay of about 32msec. This can help with CNC machines with +// problematic false triggering of their hard limit switches, but it WILL NOT fix issues with +// electrical interference on the signal cables from external sources. It's recommended to first +// use shielded signal cables with their shielding connected to ground (old USB/computer cables +// work well and are cheap to find) and wire in a low-pass circuit into each limit pin. // #define ENABLE_SOFTWARE_DEBOUNCE // Default disabled. Uncomment to enable. // --------------------------------------------------------------------------------------- diff --git a/coolant_control.c b/coolant_control.c index b849a78..593fc7c 100644 --- a/coolant_control.c +++ b/coolant_control.c @@ -18,19 +18,13 @@ along with Grbl. If not, see . */ +#include "system.h" #include "coolant_control.h" -#include "settings.h" -#include "config.h" #include "planner.h" -#include - -static uint8_t current_coolant_mode; - void coolant_init() { - current_coolant_mode = COOLANT_DISABLE; COOLANT_FLOOD_DDR |= (1 << COOLANT_FLOOD_BIT); #ifdef ENABLE_M7 COOLANT_MIST_DDR |= (1 << COOLANT_MIST_BIT); @@ -50,20 +44,16 @@ void coolant_stop() void coolant_run(uint8_t mode) { - if (mode != current_coolant_mode) - { - plan_synchronize(); // Ensure coolant turns on when specified in program. - if (mode == COOLANT_FLOOD_ENABLE) { - COOLANT_FLOOD_PORT |= (1 << COOLANT_FLOOD_BIT); - + plan_synchronize(); // Ensure coolant turns on when specified in program. + if (mode == COOLANT_FLOOD_ENABLE) { + COOLANT_FLOOD_PORT |= (1 << COOLANT_FLOOD_BIT); + #ifdef ENABLE_M7 } else if (mode == COOLANT_MIST_ENABLE) { COOLANT_MIST_PORT |= (1 << COOLANT_MIST_BIT); #endif - } else { - coolant_stop(); - } - current_coolant_mode = mode; + } else { + coolant_stop(); } } diff --git a/coolant_control.h b/coolant_control.h index 38f3b43..c19a673 100644 --- a/coolant_control.h +++ b/coolant_control.h @@ -21,12 +21,12 @@ #ifndef coolant_control_h #define coolant_control_h -#include #define COOLANT_MIST_ENABLE 2 #define COOLANT_FLOOD_ENABLE 1 #define COOLANT_DISABLE 0 // Must be zero. + void coolant_init(); void coolant_stop(); void coolant_run(uint8_t mode); diff --git a/cpu_map.h b/cpu_map.h index bbc006b..39a3bb4 100644 --- a/cpu_map.h +++ b/cpu_map.h @@ -36,9 +36,6 @@ #define SERIAL_RX USART_RX_vect #define SERIAL_UDRE USART_UDRE_vect - // Start of PWM & Stepper Enabled Spindle - // #define VARIABLE_SPINDLE // comment this out to disable PWM & Stepper on the spindle - // Define step pulse output pins. NOTE: All step bit pins must be on the same port. #define STEPPING_DDR DDRD #define STEPPING_PORT PORTD diff --git a/defaults.h b/defaults.h index cbee821..a1de3fe 100644 --- a/defaults.h +++ b/defaults.h @@ -47,7 +47,7 @@ #define DEFAULT_DIRECTION_INVERT_MASK ((1< -#include "nuts_bolts.h" -#include +#include "system.h" #include "settings.h" +#include "gcode.h" +#include "planner.h" #include "motion_control.h" #include "spindle_control.h" #include "coolant_control.h" -#include "errno.h" -#include "protocol.h" #include "report.h" // Declare gc extern struct @@ -186,9 +183,9 @@ uint8_t gc_execute_line(char *line) case 0: gc.program_flow = PROGRAM_FLOW_PAUSED; break; // Program pause case 1: break; // Optional stop not supported. Ignore. case 2: case 30: gc.program_flow = PROGRAM_FLOW_COMPLETED; break; // Program end and reset - case 3: gc.spindle_direction = 1; break; - case 4: gc.spindle_direction = -1; break; - case 5: gc.spindle_direction = 0; break; + case 3: gc.spindle_direction = SPINDLE_ENABLE_CW; break; + case 4: gc.spindle_direction = SPINDLE_ENABLE_CCW; break; + case 5: gc.spindle_direction = SPINDLE_DISABLE; break; #ifdef ENABLE_M7 case 7: gc.coolant_mode = COOLANT_MIST_ENABLE; break; #endif @@ -238,7 +235,7 @@ uint8_t gc_execute_line(char *line) case 'R': gc.arc_radius = to_millimeters(value); break; case 'S': if (value < 0) { FAIL(STATUS_INVALID_STATEMENT); } // Cannot be negative - gc.spindle_speed = value; + gc.spindle_speed = value; break; case 'T': if (value < 0) { FAIL(STATUS_INVALID_STATEMENT); } // Cannot be negative diff --git a/gcode.h b/gcode.h index aebc0ba..78d6128 100644 --- a/gcode.h +++ b/gcode.h @@ -21,8 +21,8 @@ #ifndef gcode_h #define gcode_h -#include -#include "nuts_bolts.h" + +#include "system.h" // Define modal group internal numbers for checking multiple command violations and tracking the // type of command that is called in the block. A modal group is a group of g-code commands that are @@ -70,12 +70,12 @@ typedef struct { uint8_t inches_mode; // 0 = millimeter mode, 1 = inches mode {G20, G21} uint8_t absolute_mode; // 0 = relative motion, 1 = absolute motion {G90, G91} uint8_t program_flow; // {M0, M1, M2, M30} - int8_t spindle_direction; // 1 = CW, -1 = CCW, 0 = Stop {M3, M4, M5} - uint8_t coolant_mode; // 0 = Disable, 1 = Flood Enable {M8, M9} + uint8_t coolant_mode; // 0 = Disable, 1 = Flood Enable, 2 = Mist Enable {M8, M9} + int8_t spindle_direction; // 1 = CW, 2 = CCW, 0 = Stop {M3, M4, M5} + float spindle_speed; // RPM float feed_rate; // Millimeters/min float position[N_AXIS]; // Where the interpreter considers the tool to be at this point in the code uint8_t tool; - uint16_t spindle_speed; // RPM uint8_t plane_axis_0, plane_axis_1, plane_axis_2; // The axes of the selected plane diff --git a/limits.c b/limits.c index 0abbc7d..169ca71 100644 --- a/limits.c +++ b/limits.c @@ -19,18 +19,12 @@ along with Grbl. If not, see . */ -#include -#include -#include -#include -#include "stepper.h" +#include "system.h" #include "settings.h" -#include "nuts_bolts.h" -#include "config.h" -#include "spindle_control.h" -#include "motion_control.h" -#include "planner.h" #include "protocol.h" +#include "planner.h" +#include "stepper.h" +#include "motion_control.h" #include "limits.h" #include "report.h" @@ -42,7 +36,7 @@ void limits_init() if (bit_istrue(settings.flags,BITFLAG_INVERT_LIMIT_PINS)) { LIMIT_PORT &= ~(LIMIT_MASK); // Normal low operation. Requires external pull-down. } else { - LIMIT_PORT |= (LIMIT_MASK); // Enable internal pull-up resistors. Normal high operation. + LIMIT_PORT |= (LIMIT_MASK); // Enable internal pull-up resistors. Normal high operation. } if (bit_istrue(settings.flags,BITFLAG_HARD_LIMIT_ENABLE)) { @@ -88,11 +82,9 @@ void limits_disable() // limit setting if their limits are constantly triggering after a reset and move their axes. if (sys.state != STATE_ALARM) { if (bit_isfalse(sys.execute,EXEC_ALARM)) { - #ifndef LIMIT_SWITCHES_ACTIVE_HIGH - if ((LIMIT_PIN & LIMIT_MASK) ^ LIMIT_MASK) { - #else - if (LIMIT_PIN & LIMIT_MASK) { - #endif + uint8_t bits = LIMIT_PIN; + if (bit_istrue(settings.flags,BITFLAG_INVERT_LIMIT_PINS)) { bits ^= LIMIT_MASK; } + if (bits & LIMIT_MASK) { mc_reset(); // Initiate system kill. sys.execute |= EXEC_CRIT_EVENT; // Indicate hard limit critical event } @@ -151,9 +143,9 @@ void limits_go_home(uint8_t cycle_mask, bool approach, float homing_rate) target[i] = 0.0; } } - if (bit_istrue(settings.homing_dir_mask,(1<. */ -/* A big thanks to Alden Hart of Synthetos, supplier of grblshield and TinyG, who has - been integral throughout the development of the higher level details of Grbl, as well - as being a consistent sounding board for the future of accessible and free CNC. */ - -#include -#include "config.h" +#include "system.h" +#include "serial.h" +#include "settings.h" +#include "protocol.h" +#include "gcode.h" #include "planner.h" -#include "nuts_bolts.h" #include "stepper.h" #include "spindle_control.h" #include "coolant_control.h" #include "motion_control.h" -#include "gcode.h" -#include "protocol.h" #include "limits.h" #include "report.h" -#include "settings.h" -#include "serial.h" + // Declare system global variable structure system_t sys; + int main(void) { - // Initialize system - serial_init(); // Setup serial baud rate and interrupts + // Initialize system upon power-up. + serial_init(); // Setup serial baud rate and interrupts settings_init(); // Load grbl settings from EEPROM - st_init(); // Setup stepper pins and interrupt timers - sei(); // Enable interrupts + stepper_init(); // Configure stepper pins and interrupt timers + system_init(); // Configure pinout pins and pin-change interrupt + sei(); memset(&sys, 0, sizeof(sys)); // Clear all system variables sys.abort = true; // Set abort to complete initialization @@ -65,49 +62,29 @@ int main(void) for(;;) { - // Execute system reset upon a system abort, where the main program will return to this loop. - // Once here, it is safe to re-initialize the system. At startup, the system will automatically - // reset to finish the initialization process. - if (sys.abort) { - // Reset system. - serial_reset_read_buffer(); // Clear serial read buffer - gc_init(); // Set g-code parser to default state - protocol_init(); // Clear incoming line data and execute startup lines - spindle_init(); - coolant_init(); - limits_init(); - plan_reset(); // Clear block buffer and planner variables - st_reset(); // Clear stepper subsystem variables. + // Reset the system primary functionality. + serial_reset_read_buffer(); // Clear serial read buffer + gc_init(); // Set g-code parser to default state + spindle_init(); + coolant_init(); + limits_init(); + plan_reset(); // Clear block buffer and planner variables + st_reset(); // Clear stepper subsystem variables. - // Sync cleared gcode and planner positions to current system position, which is only - // cleared upon startup, not a reset/abort. - plan_sync_position(); - gc_sync_position(); + // Sync cleared gcode and planner positions to current system position. + plan_sync_position(); + gc_sync_position(); - // Reset system variables. - sys.abort = false; - sys.execute = 0; - if (bit_istrue(settings.flags,BITFLAG_AUTO_START)) { sys.auto_start = true; } - else { sys.auto_start = false; } - - // Check for and report alarm state after a reset, error, or an initial power up. - if (sys.state == STATE_ALARM) { - report_feedback_message(MESSAGE_ALARM_LOCK); - } else { - // All systems go. Set system to ready and execute startup script. - sys.state = STATE_IDLE; // Clear all state flags. - protocol_execute_startup(); - } - } - - protocol_execute_runtime(); - - // When the serial protocol returns, there are no more characters in the serial read buffer to - // be processed and executed. This indicates that individual commands are being issued or - // streaming is finished. In either case, auto-cycle start, if enabled, any queued moves. - mc_auto_cycle_start(); - protocol_process(); // ... process the serial protocol + // Reset system variables. + sys.abort = false; + sys.execute = 0; + if (bit_istrue(settings.flags,BITFLAG_AUTO_START)) { sys.auto_start = true; } + else { sys.auto_start = false; } + + // Start main loop. Processes inputs and executes them. + // NOTE: Upon a system abort, the main loop returns and re-initializes the system. + protocol_process(); } - return 0; /* never reached */ + return 0; /* Never reached */ } diff --git a/motion_control.c b/motion_control.c index e30274a..ee1798b 100644 --- a/motion_control.c +++ b/motion_control.c @@ -20,21 +20,17 @@ along with Grbl. If not, see . */ -#include -#include -#include -#include +#include "system.h" #include "settings.h" -#include "config.h" +#include "protocol.h" #include "gcode.h" +#include "planner.h" +#include "stepper.h" #include "motion_control.h" #include "spindle_control.h" #include "coolant_control.h" -#include "nuts_bolts.h" -#include "stepper.h" -#include "planner.h" #include "limits.h" -#include "protocol.h" + // Execute linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second // unless invert_feed_rate is true. Then the feed_rate means that the motion should be completed in @@ -112,8 +108,8 @@ void mc_arc(float *position, float *target, float *offset, uint8_t axis_0, uint8 // 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)); - uint16_t segments = floor(millimeters_of_travel/ - sqrt(4*settings.arc_tolerance*(2*radius - settings.arc_tolerance)) ); + uint16_t segments = floor(0.5*millimeters_of_travel/ + sqrt(settings.arc_tolerance*(2*radius - settings.arc_tolerance)) ); if (segments) { // Multiply inverse feed_rate to compensate for the fact that this movement is approximated @@ -123,7 +119,7 @@ void mc_arc(float *position, float *target, float *offset, uint8_t axis_0, uint8 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); diff --git a/motion_control.h b/motion_control.h index a5e0371..b17e168 100644 --- a/motion_control.h +++ b/motion_control.h @@ -22,8 +22,6 @@ #ifndef motion_control_h #define motion_control_h -#include -#include "planner.h" // Execute linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second // unless invert_feed_rate is true. Then the feed_rate means that the motion should be completed in diff --git a/nuts_bolts.c b/nuts_bolts.c index 6914052..57150ef 100644 --- a/nuts_bolts.c +++ b/nuts_bolts.c @@ -19,10 +19,8 @@ along with Grbl. If not, see . */ -#include -#include "nuts_bolts.h" -#include "gcode.h" -#include "planner.h" +#include "system.h" + #define MAX_INT_DIGITS 8 // Maximum number of digits in int32 (and float) diff --git a/nuts_bolts.h b/nuts_bolts.h index 5140575..f9f08dd 100644 --- a/nuts_bolts.h +++ b/nuts_bolts.h @@ -22,13 +22,6 @@ #ifndef nuts_bolts_h #define nuts_bolts_h -#include -#include -#include -#include "config.h" -#include "defaults.h" -#include "cpu_map.h" - #define false 0 #define true 1 @@ -57,44 +50,6 @@ #define bit_istrue(x,mask) ((x & mask) != 0) #define bit_isfalse(x,mask) ((x & mask) == 0) -// Define system executor bit map. Used internally by runtime protocol as runtime command flags, -// which notifies the main program to execute the specified runtime command asynchronously. -// NOTE: The system executor uses an unsigned 8-bit volatile variable (8 flag limit.) The default -// flags are always false, so the runtime protocol only needs to check for a non-zero value to -// know when there is a runtime command to execute. -#define EXEC_STATUS_REPORT bit(0) // bitmask 00000001 -#define EXEC_CYCLE_START bit(1) // bitmask 00000010 -#define EXEC_CYCLE_STOP bit(2) // bitmask 00000100 -#define EXEC_FEED_HOLD bit(3) // bitmask 00001000 -#define EXEC_RESET bit(4) // bitmask 00010000 -#define EXEC_ALARM bit(5) // bitmask 00100000 -#define EXEC_CRIT_EVENT bit(6) // bitmask 01000000 -// #define bit(7) // bitmask 10000000 - -// Define system state bit map. The state variable primarily tracks the individual functions -// of Grbl to manage each without overlapping. It is also used as a messaging flag for -// critical events. -#define STATE_IDLE 0 // Must be zero. No flags. -#define STATE_QUEUED bit(0) // Indicates buffered blocks, awaiting cycle start. -#define STATE_CYCLE bit(1) // Cycle is running -#define STATE_HOLD bit(2) // Executing feed hold -#define STATE_HOMING bit(3) // Performing homing cycle -#define STATE_ALARM bit(4) // In alarm state. Locks out all g-code processes. Allows settings access. -#define STATE_CHECK_MODE bit(5) // G-code check mode. Locks out planner and motion only. -// #define STATE_JOG bit(6) // Jogging mode is unique like homing. - -// Define global system variables -typedef struct { - uint8_t abort; // System abort flag. Forces exit back to main loop for reset. - uint8_t state; // Tracks the current state of Grbl. - volatile uint8_t execute; // Global system runtime executor bitflag variable. See EXEC bitmasks. - uint8_t homing_axis_lock; - int32_t position[N_AXIS]; // Real-time machine (aka home) position vector in steps. - // NOTE: This may need to be a volatile variable, if problems arise. - uint8_t auto_start; // Planner auto-start flag. Toggled off during feed hold. Defaulted by settings. -} system_t; -extern system_t sys; - // 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 float_ptr is // a pointer to the result variable. Returns true when it succeeds diff --git a/planner.c b/planner.c index 143837a..9e435c7 100644 --- a/planner.c +++ b/planner.c @@ -22,14 +22,12 @@ /* The ring buffer implementation gleaned from the wiring_serial library by David A. Mellis. */ -#include -#include +#include "system.h" #include "planner.h" -#include "nuts_bolts.h" +#include "protocol.h" #include "stepper.h" #include "settings.h" -#include "config.h" -#include "protocol.h" + #define SOME_LARGE_VALUE 1.0E+38 // Used by rapids and acceleration maximization calculations. Just needs // to be larger than any feasible (mm/min)^2 or mm/sec^2 value. diff --git a/planner.h b/planner.h index 1cc4309..99298ed 100644 --- a/planner.h +++ b/planner.h @@ -21,7 +21,8 @@ #ifndef planner_h #define planner_h -#include "nuts_bolts.h" + +#include "system.h" // The number of linear motions that can be in the plan at any give time #ifndef BLOCK_BUFFER_SIZE diff --git a/print.c b/print.c index 0e2f9b8..03e261d 100644 --- a/print.c +++ b/print.c @@ -22,12 +22,11 @@ /* This code was initially inspired by the wiring_serial module by David A. Mellis which used to be a part of the Arduino project. */ - -#include -#include "config.h" +#include "system.h" #include "serial.h" #include "settings.h" + void printString(const char *s) { while (*s) diff --git a/protocol.c b/protocol.c index 54c959f..ade1df0 100644 --- a/protocol.c +++ b/protocol.c @@ -1,5 +1,5 @@ /* - protocol.c - the serial protocol master control unit + protocol.c - controls Grbl execution procedures Part of Grbl Copyright (c) 2011-2014 Sungeun K. Jeon @@ -19,78 +19,19 @@ along with Grbl. If not, see . */ -#include -#include +#include "system.h" +#include "serial.h" +#include "settings.h" #include "protocol.h" #include "gcode.h" -#include "serial.h" -#include "print.h" -#include "settings.h" -#include "config.h" -#include "nuts_bolts.h" #include "stepper.h" -#include "report.h" #include "motion_control.h" +#include "report.h" + static char line[LINE_BUFFER_SIZE]; // Line to be executed. Zero-terminated. -static uint8_t char_counter; // Last character counter in line variable. -static uint8_t iscomment; // Comment/block delete flag for processor to ignore comment characters. -static void protocol_reset_line_buffer() -{ - char_counter = 0; - iscomment = false; -} - - -void protocol_init() -{ - protocol_reset_line_buffer(); // Reset line input - report_init_message(); // Welcome message - - PINOUT_DDR &= ~(PINOUT_MASK); // Set as input pins - PINOUT_PORT |= PINOUT_MASK; // Enable internal pull-up resistors. Normal high operation. - PINOUT_PCMSK |= PINOUT_MASK; // Enable specific pins of the Pin Change Interrupt - PCICR |= (1 << PINOUT_INT); // Enable Pin Change Interrupt -} - - -// Executes user startup script, if stored. -void protocol_execute_startup() -{ - uint8_t n; - for (n=0; n < N_STARTUP_LINE; n++) { - if (!(settings_read_startup_line(n, line))) { - report_status_message(STATUS_SETTING_READ_FAIL); - } else { - if (line[0] != 0) { - printString(line); // Echo startup line to indicate execution. - report_status_message(gc_execute_line(line)); - } - } - } -} - - -// Pin change interrupt for pin-out commands, i.e. cycle start, feed hold, and reset. Sets -// only the runtime command execute variable to have the main program execute these when -// its ready. This works exactly like the character-based runtime commands when picked off -// directly from the incoming serial data stream. -ISR(PINOUT_INT_vect) -{ - // Enter only if any pinout pin is actively low. - if ((PINOUT_PIN & PINOUT_MASK) ^ PINOUT_MASK) { - if (bit_isfalse(PINOUT_PIN,bit(PIN_RESET))) { - mc_reset(); - } else if (bit_isfalse(PINOUT_PIN,bit(PIN_FEED_HOLD))) { - sys.execute |= EXEC_FEED_HOLD; - } else if (bit_isfalse(PINOUT_PIN,bit(PIN_CYCLE_START))) { - sys.execute |= EXEC_CYCLE_START; - } - } -} - // Executes run-time commands, when required. This is called from various check points in the main // program, primarily where there may be a while loop waiting for a buffer to clear space or any // point where the execution time from the last check point may be more than a fraction of a second. @@ -181,188 +122,111 @@ void protocol_execute_runtime() // Directs and executes one line of formatted input from protocol_process. While mostly -// incoming streaming g-code blocks, this also executes Grbl internal commands, such as -// settings, initiating the homing cycle, and toggling switch states. This differs from -// the runtime command module by being susceptible to when Grbl is ready to execute the -// next line during a cycle, so for switches like block delete, the switch only effects -// the lines that are processed afterward, not necessarily real-time during a cycle, -// since there are motions already stored in the buffer. However, this 'lag' should not -// be an issue, since these commands are not typically used during a cycle. -uint8_t protocol_execute_line(char *line) -{ - // Grbl internal command and parameter lines are of the form '$4=374.3' or '$' for help - if(line[0] == '$') { - uint8_t char_counter = 1; - uint8_t helper_var = 0; // Helper variable - float parameter, value; - switch( line[char_counter] ) { - case '#' : // Print gcode parameters - if ( line[++char_counter] != 0 ) { return(STATUS_UNSUPPORTED_STATEMENT); } - else { report_gcode_parameters(); } - break; - case 'G' : // Prints gcode parser state - if ( line[++char_counter] != 0 ) { return(STATUS_UNSUPPORTED_STATEMENT); } - else { report_gcode_modes(); } - break; -// case 'J' : break; // Jogging methods - // TODO: Here jogging can be placed for execution as a seperate subprogram. It does not need to be - // susceptible to other runtime commands except for e-stop. The jogging function is intended to - // be a basic toggle on/off with controlled acceleration and deceleration to prevent skipped - // steps. The user would supply the desired feedrate, axis to move, and direction. Toggle on would - // start motion and toggle off would initiate a deceleration to stop. One could 'feather' the - // motion by repeatedly toggling to slow the motion to the desired location. Location data would - // need to be updated real-time and supplied to the user through status queries. - // More controlled exact motions can be taken care of by inputting G0 or G1 commands, which are - // handled by the planner. It would be possible for the jog subprogram to insert blocks into the - // block buffer without having the planner plan them. It would need to manage de/ac-celerations - // on its own carefully. This approach could be effective and possibly size/memory efficient. - default : - // Block any system command that requires the state as IDLE/ALARM. (i.e. EEPROM, homing) - if ( !(sys.state == STATE_IDLE || sys.state == STATE_ALARM) ) { return(STATUS_IDLE_ERROR); } - switch( line[char_counter] ) { - case 0 : report_grbl_help(); break; - case '$' : // Prints Grbl settings - if ( line[++char_counter] != 0 ) { return(STATUS_UNSUPPORTED_STATEMENT); } - else { report_grbl_settings(); } - break; - case 'C' : // Set check g-code mode - if ( line[++char_counter] != 0 ) { return(STATUS_UNSUPPORTED_STATEMENT); } - // Perform reset when toggling off. Check g-code mode should only work if Grbl - // is idle and ready, regardless of alarm locks. This is mainly to keep things - // simple and consistent. - if ( sys.state == STATE_CHECK_MODE ) { - mc_reset(); - report_feedback_message(MESSAGE_DISABLED); - } else { - if (sys.state) { return(STATUS_IDLE_ERROR); } - sys.state = STATE_CHECK_MODE; - report_feedback_message(MESSAGE_ENABLED); - } - break; - case 'X' : // Disable alarm lock - if ( line[++char_counter] != 0 ) { return(STATUS_UNSUPPORTED_STATEMENT); } - if (sys.state == STATE_ALARM) { - report_feedback_message(MESSAGE_ALARM_UNLOCK); - sys.state = STATE_IDLE; - // Don't run startup script. Prevents stored moves in startup from causing accidents. - } - break; - case 'H' : // Perform homing cycle - if (bit_istrue(settings.flags,BITFLAG_HOMING_ENABLE)) { - // Only perform homing if Grbl is idle or alarm. - mc_homing_cycle(); - if (!sys.abort) { protocol_execute_startup(); } // Execute startup scripts after successful homing. - } else { return(STATUS_SETTING_DISABLED); } - break; - case 'I' : // Print or store build info. - if ( line[++char_counter] == 0 ) { - if (!(settings_read_build_info(line))) { - report_status_message(STATUS_SETTING_READ_FAIL); - } else { - report_build_info(line); - } - } else { // Store startup line - if(line[char_counter++] != '=') { return(STATUS_UNSUPPORTED_STATEMENT); } - helper_var = char_counter; // Set helper variable as counter to start of user info line. - do { - line[char_counter-helper_var] = line[char_counter]; - } while (line[char_counter++] != 0); - settings_store_build_info(line); - } - break; - case 'N' : // Startup lines. - if ( line[++char_counter] == 0 ) { // Print startup lines - for (helper_var=0; helper_var < N_STARTUP_LINE; helper_var++) { - if (!(settings_read_startup_line(helper_var, line))) { - report_status_message(STATUS_SETTING_READ_FAIL); - } else { - report_startup_line(helper_var,line); - } - } - break; - } else { // Store startup line - helper_var = true; // Set helper_var to flag storing method. - // No break. Continues into default: to read remaining command characters. - } - default : // Storing setting methods - if(!read_float(line, &char_counter, ¶meter)) { return(STATUS_BAD_NUMBER_FORMAT); } - if(line[char_counter++] != '=') { return(STATUS_UNSUPPORTED_STATEMENT); } - if (helper_var) { // Store startup line - // Prepare sending gcode block to gcode parser by shifting all characters - helper_var = char_counter; // Set helper variable as counter to start of gcode block - do { - line[char_counter-helper_var] = line[char_counter]; - } while (line[char_counter++] != 0); - // Execute gcode block to ensure block is valid. - helper_var = gc_execute_line(line); // Set helper_var to returned status code. - if (helper_var) { return(helper_var); } - else { - helper_var = trunc(parameter); // Set helper_var to int value of parameter - settings_store_startup_line(helper_var,line); - } - } else { // Store global setting. - if(!read_float(line, &char_counter, &value)) { return(STATUS_BAD_NUMBER_FORMAT); } - if(line[char_counter] != 0) { return(STATUS_UNSUPPORTED_STATEMENT); } - return(settings_store_global_setting(parameter, value)); - } - } - } - return(STATUS_OK); // If '$' command makes it to here, then everything's ok. +// incoming streaming g-code blocks, this also directs and executes Grbl internal commands, +// such as settings, initiating the homing cycle, and toggling switch states. +// TODO: Eventually re-organize this function to more cleanly organize order of operations, +// which will hopefully reduce some of the current spaghetti logic and dynamic memory usage. +static void protocol_execute_line(char *line) +{ + protocol_execute_runtime(); // Runtime command check point. + if (sys.abort) { return; } // Bail to calling function upon system abort + uint8_t status; + if (line[0] == 0) { + // Empty or comment line. Send status message for syncing purposes. + status = STATUS_OK; + + } else if (line[0] == '$') { + // Grbl '$' system command + status = system_execute_line(line); + } else { - return(gc_execute_line(line)); // Everything else is gcode + // Everything else is gcode. Send to g-code parser! + // TODO: Separate the parsing from the g-code execution. Need to re-write the parser + // completely to do this. First parse the line completely, checking for modal group + // errors and storing all of the g-code words. Then, send the stored g-code words to + // a separate g-code executor. This will be more in-line with actual g-code protocol. + status = gc_execute_line(line); + } + + report_status_message(status); } -// Process and report status one line of incoming serial data. Performs an initial filtering -// by removing spaces and comments and capitalizing all letters. void protocol_process() { + // ------------------------------------------------------------ + // Complete initialization procedures upon a power-up or reset. + // ------------------------------------------------------------ + + // Print welcome message + report_init_message(); + + // Check for and report alarm state after a reset, error, or an initial power up. + if (sys.state == STATE_ALARM) { + report_feedback_message(MESSAGE_ALARM_LOCK); + } else { + // All systems go! + sys.state = STATE_IDLE; // Set system to ready. Clear all state flags. + system_execute_startup(line); // Execute startup script. + } + + // ------------------------------------------------------------------------------ + // Main loop! Upon a system abort, this exits back to main() to reset the system. + // ------------------------------------------------------------------------------ + + uint8_t iscomment = false; + uint8_t char_counter = 0; uint8_t c; - while((c = serial_read()) != SERIAL_NO_DATA) { - if ((c == '\n') || (c == '\r')) { // End of line reached + for (;;) { - // Runtime command check point before executing line. Prevent any furthur line executions. - // NOTE: If there is no line, this function should quickly return to the main program when - // the buffer empties of non-executable data. - protocol_execute_runtime(); - if (sys.abort) { return; } // Bail to main program upon system abort - - if (char_counter > 0) {// Line is complete. Then execute! - line[char_counter] = 0; // Terminate string - report_status_message(protocol_execute_line(line)); - } else { - // Empty or comment line. Skip block. - report_status_message(STATUS_OK); // Send status message for syncing purposes. - } - protocol_reset_line_buffer(); - - } else { - if (iscomment) { - // Throw away all comment characters - if (c == ')') { - // End of comment. Resume line. - iscomment = false; - } + // Process one line of incoming serial data, as the data becomes available. Performs an + // initial filtering by removing spaces and comments and capitalizing all letters. + while((c = serial_read()) != SERIAL_NO_DATA) { + if ((c == '\n') || (c == '\r')) { // End of line reached + line[char_counter] = 0; // Set string termination character. + protocol_execute_line(line); // Line is complete. Execute it! + iscomment = false; + char_counter = 0; } else { - if (c <= ' ') { - // Throw away whitepace and control characters - } else if (c == '/') { - // Block delete not supported. Ignore character. - } else if (c == '(') { - // Enable comments flag and ignore all characters until ')' or EOL. - iscomment = true; - } else if (char_counter >= LINE_BUFFER_SIZE-1) { - // Detect line buffer overflow. Report error and reset line buffer. - report_status_message(STATUS_OVERFLOW); - protocol_reset_line_buffer(); - } else if (c >= 'a' && c <= 'z') { // Upcase lowercase - line[char_counter++] = c-'a'+'A'; + if (iscomment) { + // Throw away all comment characters + if (c == ')') { + // End of comment. Resume line. + iscomment = false; + } } else { - line[char_counter++] = c; + if (c <= ' ') { + // Throw away whitepace and control characters + } else if (c == '/') { + // Block delete not supported. Ignore character. + } else if (c == '(') { + // Enable comments flag and ignore all characters until ')' or EOL. + iscomment = true; + } else if (char_counter >= LINE_BUFFER_SIZE-1) { + // Detect line buffer overflow. Report error and reset line buffer. + report_status_message(STATUS_OVERFLOW); + iscomment = false; + char_counter = 0; + } else if (c >= 'a' && c <= 'z') { // Upcase lowercase + line[char_counter++] = c-'a'+'A'; + } else { + line[char_counter++] = c; + } } } } + + protocol_execute_runtime(); // Runtime command check point. + if (sys.abort) { return; } // Bail to main() program loop to reset system. + + // If there are no more characters in the serial read buffer to be processed and executed, + // this indicates that g-code streaming has either filled the planner buffer or has + // completed. In either case, auto-cycle start, if enabled, any queued moves. + mc_auto_cycle_start(); + } + + return; /* Never reached */ } diff --git a/protocol.h b/protocol.h index b6267d4..9107d1d 100644 --- a/protocol.h +++ b/protocol.h @@ -1,5 +1,5 @@ /* - protocol.h - the serial protocol master control unit + protocol.h - controls Grbl execution procedures Part of Grbl Copyright (c) 2011-2014 Sungeun K. Jeon @@ -21,32 +21,21 @@ #ifndef protocol_h #define protocol_h -#include - // Line buffer size from the serial input stream to be executed. // NOTE: Not a problem except for extreme cases, but the line buffer size can be too small // and g-code blocks can get truncated. Officially, the g-code standards support up to 256 // characters. In future versions, this will be increased, when we know how much extra -// memory space we can invest into here or we re-write the g-code parser not to have his +// memory space we can invest into here or we re-write the g-code parser not to have this // buffer. #ifndef LINE_BUFFER_SIZE #define LINE_BUFFER_SIZE 70 #endif -// Initialize the serial protocol -void protocol_init(); - -// Read command lines from the serial port and execute them as they -// come in. Blocks until the serial buffer is emptied. -void protocol_process(); - -// Executes one line of input according to protocol -uint8_t protocol_execute_line(char *line); - // Checks and executes a runtime command at various stop points in main program void protocol_execute_runtime(); -// Execute the startup script lines stored in EEPROM upon initialization -void protocol_execute_startup(); +// Starts Grbl main loop. It handles all incoming characters from the serial port and executes +// them as they complete. It is also responsible for finishing the initialization procedures. +void protocol_process(); #endif diff --git a/report.c b/report.c index 1796c8c..ee4de61 100644 --- a/report.c +++ b/report.c @@ -26,11 +26,10 @@ methods to accomodate their needs. */ -#include +#include "system.h" #include "report.h" #include "print.h" #include "settings.h" -#include "nuts_bolts.h" #include "gcode.h" #include "coolant_control.h" @@ -163,9 +162,9 @@ void report_grbl_settings() { printPgmString(PSTR(" (z max travel, mm)\r\n$12=")); printInteger(settings.pulse_microseconds); printPgmString(PSTR(" (step pulse, usec)\r\n$13=")); printFloat(settings.default_feed_rate); printPgmString(PSTR(" (default feed, mm/min)\r\n$14=")); printInteger(settings.step_invert_mask); - printPgmString(PSTR(" (step port invert mask, int:")); print_uint8_base2(settings.step_invert_mask); + printPgmString(PSTR(" (step port invert mask:")); print_uint8_base2(settings.step_invert_mask); printPgmString(PSTR(")\r\n$15=")); printInteger(settings.dir_invert_mask); - printPgmString(PSTR(" (dir port invert mask, int:")); print_uint8_base2(settings.dir_invert_mask); + printPgmString(PSTR(" (dir port invert mask:")); print_uint8_base2(settings.dir_invert_mask); printPgmString(PSTR(")\r\n$16=")); printInteger(settings.stepper_idle_lock_time); printPgmString(PSTR(" (step idle delay, msec)\r\n$17=")); printFloat(settings.junction_deviation); printPgmString(PSTR(" (junction deviation, mm)\r\n$18=")); printFloat(settings.arc_tolerance); @@ -178,7 +177,7 @@ void report_grbl_settings() { printPgmString(PSTR(" (soft limits, bool)\r\n$25=")); printInteger(bit_istrue(settings.flags,BITFLAG_HARD_LIMIT_ENABLE)); printPgmString(PSTR(" (hard limits, bool)\r\n$26=")); printInteger(bit_istrue(settings.flags,BITFLAG_HOMING_ENABLE)); printPgmString(PSTR(" (homing cycle, bool)\r\n$27=")); printInteger(settings.homing_dir_mask); - printPgmString(PSTR(" (homing dir invert mask, int:")); print_uint8_base2(settings.homing_dir_mask); + printPgmString(PSTR(" (homing dir invert mask:")); print_uint8_base2(settings.homing_dir_mask); printPgmString(PSTR(")\r\n$28=")); printFloat(settings.homing_feed_rate); printPgmString(PSTR(" (homing feed, mm/min)\r\n$29=")); printFloat(settings.homing_seek_rate); printPgmString(PSTR(" (homing seek, mm/min)\r\n$30=")); printInteger(settings.homing_debounce_delay); diff --git a/serial.c b/serial.c index 382ee41..29e361c 100644 --- a/serial.c +++ b/serial.c @@ -23,11 +23,12 @@ used to be a part of the Arduino project. */ #include +#include "system.h" #include "serial.h" -#include "config.h" #include "motion_control.h" #include "protocol.h" + uint8_t rx_buffer[RX_BUFFER_SIZE]; uint8_t rx_buffer_head = 0; volatile uint8_t rx_buffer_tail = 0; @@ -36,6 +37,7 @@ uint8_t tx_buffer[TX_BUFFER_SIZE]; uint8_t tx_buffer_head = 0; volatile uint8_t tx_buffer_tail = 0; + #ifdef ENABLE_XONXOFF volatile uint8_t flow_ctrl = XON_SENT; // Flow control state variable @@ -49,6 +51,7 @@ volatile uint8_t tx_buffer_tail = 0; } #endif + void serial_init() { // Set baud rate @@ -72,6 +75,7 @@ void serial_init() // defaults to 8-bit, no parity, 1 stop bit } + void serial_write(uint8_t data) { // Calculate next head uint8_t next_head = tx_buffer_head + 1; @@ -90,6 +94,7 @@ void serial_write(uint8_t data) { UCSR0B |= (1 << UDRIE0); } + // Data Register Empty Interrupt handler ISR(SERIAL_UDRE) { @@ -119,6 +124,7 @@ ISR(SERIAL_UDRE) if (tail == tx_buffer_head) { UCSR0B &= ~(1 << UDRIE0); } } + uint8_t serial_read() { uint8_t tail = rx_buffer_tail; // Temporary rx_buffer_tail (to optimize for volatile) @@ -142,6 +148,7 @@ uint8_t serial_read() } } + ISR(SERIAL_RX) { uint8_t data = UDR0; @@ -174,6 +181,7 @@ ISR(SERIAL_RX) } } + void serial_reset_read_buffer() { rx_buffer_tail = rx_buffer_head; diff --git a/serial.h b/serial.h index e768b44..74b429b 100644 --- a/serial.h +++ b/serial.h @@ -25,7 +25,6 @@ #ifndef serial_h #define serial_h -#include "nuts_bolts.h" #ifndef RX_BUFFER_SIZE #define RX_BUFFER_SIZE 128 diff --git a/settings.c b/settings.c index a312ee4..76306a3 100644 --- a/settings.c +++ b/settings.c @@ -19,29 +19,15 @@ along with Grbl. If not, see . */ -#include -#include "protocol.h" -#include "report.h" -#include "stepper.h" -#include "nuts_bolts.h" +#include "system.h" #include "settings.h" #include "eeprom.h" +#include "protocol.h" +#include "report.h" #include "limits.h" settings_t settings; -// Version 4 outdated settings record -typedef struct { - float steps_per_mm[N_AXIS]; - uint8_t microsteps; - uint8_t pulse_microseconds; - float default_feed_rate; - uint8_t invert_mask; - float mm_per_arc_segment; - float acceleration; - float junction_deviation; -} settings_v4_t; - // Method to store startup lines into EEPROM void settings_store_startup_line(uint8_t n, char *line) @@ -50,12 +36,14 @@ void settings_store_startup_line(uint8_t n, char *line) memcpy_to_eeprom_with_checksum(addr,(char*)line, LINE_BUFFER_SIZE); } + // Method to store build info into EEPROM void settings_store_build_info(char *line) { memcpy_to_eeprom_with_checksum(EEPROM_ADDR_BUILD_INFO,(char*)line, LINE_BUFFER_SIZE); } + // Method to store coord data parameters into EEPROM void settings_write_coord_data(uint8_t coord_select, float *coord_data) { @@ -63,6 +51,7 @@ void settings_write_coord_data(uint8_t coord_select, float *coord_data) memcpy_to_eeprom_with_checksum(addr,(char*)coord_data, sizeof(float)*N_AXIS); } + // Method to store Grbl global settings struct and version number into EEPROM void write_global_settings() { @@ -70,27 +59,24 @@ void write_global_settings() memcpy_to_eeprom_with_checksum(EEPROM_ADDR_GLOBAL, (char*)&settings, sizeof(settings_t)); } + // Method to reset Grbl global settings back to defaults. -void settings_reset(bool reset_all) { - // Reset all settings or only the migration settings to the new version. - if (reset_all) { - settings.steps_per_mm[X_AXIS] = DEFAULT_X_STEPS_PER_MM; - settings.steps_per_mm[Y_AXIS] = DEFAULT_Y_STEPS_PER_MM; - settings.steps_per_mm[Z_AXIS] = DEFAULT_Z_STEPS_PER_MM; - settings.pulse_microseconds = DEFAULT_STEP_PULSE_MICROSECONDS; - settings.default_feed_rate = DEFAULT_FEEDRATE; - settings.max_rate[X_AXIS] = DEFAULT_X_MAX_RATE; - settings.max_rate[Y_AXIS] = DEFAULT_Y_MAX_RATE; - settings.max_rate[Z_AXIS] = DEFAULT_Z_MAX_RATE; - settings.acceleration[X_AXIS] = DEFAULT_X_ACCELERATION; - settings.acceleration[Y_AXIS] = DEFAULT_Y_ACCELERATION; - settings.acceleration[Z_AXIS] = DEFAULT_Z_ACCELERATION; - settings.arc_tolerance = DEFAULT_ARC_TOLERANCE; - settings.step_invert_mask = DEFAULT_STEPPING_INVERT_MASK; - settings.dir_invert_mask = DEFAULT_DIRECTION_INVERT_MASK; - settings.junction_deviation = DEFAULT_JUNCTION_DEVIATION; - } - // New settings since last version +void settings_reset() { + settings.steps_per_mm[X_AXIS] = DEFAULT_X_STEPS_PER_MM; + settings.steps_per_mm[Y_AXIS] = DEFAULT_Y_STEPS_PER_MM; + settings.steps_per_mm[Z_AXIS] = DEFAULT_Z_STEPS_PER_MM; + settings.pulse_microseconds = DEFAULT_STEP_PULSE_MICROSECONDS; + settings.default_feed_rate = DEFAULT_FEEDRATE; + settings.max_rate[X_AXIS] = DEFAULT_X_MAX_RATE; + settings.max_rate[Y_AXIS] = DEFAULT_Y_MAX_RATE; + settings.max_rate[Z_AXIS] = DEFAULT_Z_MAX_RATE; + settings.acceleration[X_AXIS] = DEFAULT_X_ACCELERATION; + settings.acceleration[Y_AXIS] = DEFAULT_Y_ACCELERATION; + settings.acceleration[Z_AXIS] = DEFAULT_Z_ACCELERATION; + settings.arc_tolerance = DEFAULT_ARC_TOLERANCE; + settings.step_invert_mask = DEFAULT_STEPPING_INVERT_MASK; + settings.dir_invert_mask = DEFAULT_DIRECTION_INVERT_MASK; + settings.junction_deviation = DEFAULT_JUNCTION_DEVIATION; settings.flags = 0; if (DEFAULT_REPORT_INCHES) { settings.flags |= BITFLAG_REPORT_INCHES; } if (DEFAULT_AUTO_START) { settings.flags |= BITFLAG_AUTO_START; } @@ -112,13 +98,14 @@ void settings_reset(bool reset_all) { write_global_settings(); } + // Reads startup line from EEPROM. Updated pointed line string data. uint8_t settings_read_startup_line(uint8_t n, char *line) { uint16_t addr = n*(LINE_BUFFER_SIZE+1)+EEPROM_ADDR_STARTUP_BLOCK; if (!(memcpy_from_eeprom_with_checksum((char*)line, addr, LINE_BUFFER_SIZE))) { // Reset line with default value - line[0] = 0; + line[0] = 0; // Empty line settings_store_startup_line(n, line); return(false); } else { @@ -126,12 +113,13 @@ uint8_t settings_read_startup_line(uint8_t n, char *line) } } + // Reads startup line from EEPROM. Updated pointed line string data. uint8_t settings_read_build_info(char *line) { if (!(memcpy_from_eeprom_with_checksum((char*)line, EEPROM_ADDR_BUILD_INFO, LINE_BUFFER_SIZE))) { // Reset line with default value - line[0] = 0; + line[0] = 0; // Empty line settings_store_build_info(line); return(false); } else { @@ -139,6 +127,7 @@ uint8_t settings_read_build_info(char *line) } } + // Read selected coordinate data from EEPROM. Updates pointed coord_data value. uint8_t settings_read_coord_data(uint8_t coord_select, float *coord_data) { @@ -153,26 +142,18 @@ uint8_t settings_read_coord_data(uint8_t coord_select, float *coord_data) } } + // Reads Grbl global settings struct from EEPROM. uint8_t read_global_settings() { // Check version-byte of eeprom uint8_t version = eeprom_get_char(0); - if (version == SETTINGS_VERSION) { // Read settings-record and check checksum if (!(memcpy_from_eeprom_with_checksum((char*)&settings, EEPROM_ADDR_GLOBAL, sizeof(settings_t)))) { return(false); } } else { - if (version <= 4) { - // Migrate from settings version 4 to current version. - if (!(memcpy_from_eeprom_with_checksum((char*)&settings, 1, sizeof(settings_v4_t)))) { - return(false); - } - settings_reset(false); // Old settings ok. Write new settings only. - } else { - return(false); - } + return(false); } return(true); } @@ -180,9 +161,9 @@ uint8_t read_global_settings() { // A helper method to set settings from command line uint8_t settings_store_global_setting(int parameter, float value) { + if (value < 0.0) { return(STATUS_SETTING_VALUE_NEG); } switch(parameter) { case 0: case 1: case 2: - if (value <= 0.0) { return(STATUS_SETTING_VALUE_NEG); } settings.steps_per_mm[parameter] = value; break; case 3: settings.max_rate[X_AXIS] = value; break; case 4: settings.max_rate[Y_AXIS] = value; break; @@ -249,11 +230,12 @@ uint8_t settings_store_global_setting(int parameter, float value) { return(STATUS_OK); } + // Initialize the config subsystem void settings_init() { if(!read_global_settings()) { report_status_message(STATUS_SETTING_READ_FAIL); - settings_reset(true); + settings_reset(); report_grbl_settings(); } // Read all parameter data into a dummy variable. If error, reset to zero, otherwise do nothing. diff --git a/settings.h b/settings.h index 05840bb..6ff54be 100644 --- a/settings.h +++ b/settings.h @@ -22,15 +22,15 @@ #ifndef settings_h #define settings_h -#include -#include "nuts_bolts.h" +#include "system.h" + #define GRBL_VERSION "0.9c" -#define GRBL_VERSION_BUILD "20131231" +#define GRBL_VERSION_BUILD "20140110" // Version of the EEPROM data. Will be used to migrate existing data from older versions of Grbl // when firmware is upgraded. Always stored in byte 0 of eeprom -#define SETTINGS_VERSION 57 +#define SETTINGS_VERSION 6 // Define bit flag masks for the boolean settings in settings.flag. #define BITFLAG_REPORT_INCHES bit(0) diff --git a/spindle_control.c b/spindle_control.c index 70e819e..e97c3f7 100644 --- a/spindle_control.c +++ b/spindle_control.c @@ -19,17 +19,13 @@ along with Grbl. If not, see . */ -#include "settings.h" +#include "system.h" #include "spindle_control.h" #include "planner.h" -static uint8_t current_direction; -static uint16_t current_rpm; - void spindle_init() -{ - current_direction = 0; +{ SPINDLE_DIRECTION_DDR |= (1< 0) { - SPINDLE_DIRECTION_PORT &= ~(1< SPINDLE_MAX_RPM) { rpm = SPINDLE_MAX_RPM; } // Prevent overflow. - uint8_t current_pwm = floor((((float) rpm / (float) SPINDLE_MAX_RPM ) * 255.0) + 0.5); - OCR_REGISTER = current_pwm; - - #ifndef CPU_MAP_ATMEGA328P // On the Uno, spindle enable and PWM are shared. - SPINDLE_ENABLE_PORT |= (1< SPINDLE_RPM_RANGE ) { rpm = SPINDLE_RPM_RANGE; } // Prevent uint8 overflow + uint8_t current_pwm = floor( rpm*(255.0/SPINDLE_RPM_RANGE) + 0.5); + OCR_REGISTER = current_pwm; + + #ifndef CPU_MAP_ATMEGA328P // On the Uno, spindle enable and PWM are shared. SPINDLE_ENABLE_PORT |= (1< + +#define SPINDLE_DISABLE 0 // Must be zero. +#define SPINDLE_ENABLE_CW 1 +#define SPINDLE_ENABLE_CCW 2 + // Initializes spindle pins and hardware PWM, if enabled. void spindle_init(); // Sets spindle direction and spindle rpm via PWM, if enabled. -void spindle_run(int8_t direction, uint16_t rpm); +void spindle_run(uint8_t direction, float rpm); // Kills spindle. void spindle_stop(); diff --git a/stepper.c b/stepper.c index 7792896..4618ef8 100644 --- a/stepper.c +++ b/stepper.c @@ -19,12 +19,11 @@ along with Grbl. If not, see . */ -#include +#include "system.h" +#include "nuts_bolts.h" #include "stepper.h" -#include "config.h" #include "settings.h" #include "planner.h" -#include "nuts_bolts.h" // Some useful constants. @@ -43,7 +42,7 @@ // NOTE: Current settings are set to overdrive the ISR to no more than 16kHz, balancing CPU overhead // and timer accuracy. Do not alter these settings unless you know what you are doing. #define MAX_AMASS_LEVEL 3 -// AMASS_LEVEL0: Normal operation. No AMASS. No upper cutoff frequency. +// AMASS_LEVEL0: Normal operation. No AMASS. No upper cutoff frequency. Starts at LEVEL1 cutoff frequency. #define AMASS_LEVEL1 (F_CPU/8000) // Over-drives ISR (x2). Defined as F_CPU/(Cutoff frequency in Hz) #define AMASS_LEVEL2 (F_CPU/4000) // Over-drives ISR (x4) #define AMASS_LEVEL3 (F_CPU/2000) // Over-drives ISR (x8) @@ -182,12 +181,10 @@ static st_prep_t prep; // enabled. Startup init and limits call this function but shouldn't start the cycle. void st_wake_up() { - // Enable steppers by resetting the stepper disable port - if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { - STEPPERS_DISABLE_PORT |= (1<millimeters; float time_var = dt_max; // Time worker variable float mm_var; // mm-Distance worker variable @@ -676,8 +673,8 @@ void st_prep_buffer() switch (prep.ramp_type) { case RAMP_ACCEL: // NOTE: Acceleration ramp only computes during first do-while loop. - speed_var = pl_block->acceleration*dt_max; - mm_remaining -= dt_max*(prep.current_speed + 0.5*speed_var); + speed_var = pl_block->acceleration*time_var; + mm_remaining -= time_var*(prep.current_speed + 0.5*speed_var); if (mm_remaining < prep.accelerate_until) { // End of acceleration ramp. // Acceleration-cruise, acceleration-deceleration ramp junction, or end of block. mm_remaining = prep.accelerate_until; // NOTE: 0.0 at EOB @@ -718,10 +715,14 @@ void st_prep_buffer() } dt += time_var; // Add computed ramp time to total segment time. if (dt < dt_max) { time_var = dt_max - dt; } // **Incomplete** At ramp junction. - else if (mm_remaining > prep.minimum_mm) { // Check for slow segments with zero steps. - dt_max += DT_SEGMENT; // Increase segment time to ensure at least one step in segment. - time_var = dt_max - dt; - } else { break; } // **Complete** Exit loop. Segment execution time maxed. + else { + if (mm_remaining > prep.minimum_mm) { // Check for slow segments with zero steps. + dt_max += DT_SEGMENT; // Increase segment time to ensure at least one step in segment. + time_var = dt_max - dt; + } else { + break; // **Complete** Exit loop. Segment execution time maxed. + } + } } while (mm_remaining > prep.mm_complete); // **Complete** Exit loop. Profile complete. /* ----------------------------------------------------------------------------------- @@ -745,7 +746,7 @@ void st_prep_buffer() #ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING // Compute step timing and multi-axis smoothing level. - // NOTE: Only one prescalar is required with AMASS enabled. + // NOTE: AMASS overdrives the timer with each level, so only one prescalar is required. if (cycles < AMASS_LEVEL1) { prep_segment->amass_level = 0; } else { if (cycles < AMASS_LEVEL2) { prep_segment->amass_level = 1; } diff --git a/stepper.h b/stepper.h index a72a565..fe9323f 100644 --- a/stepper.h +++ b/stepper.h @@ -27,7 +27,7 @@ #endif // Initialize and setup the stepper motor subsystem -void st_init(); +void stepper_init(); // Enable steppers, but cycle does not start unless called by motion control or runtime command. void st_wake_up(); @@ -47,7 +47,7 @@ void st_cycle_reinitialize(); // Initiates a feed hold of the running program void st_feed_hold(); -// Reloads step segment buffer. Called continuously by runtime execution protocol. +// Reloads step segment buffer. Called continuously by runtime execution system. void st_prep_buffer(); // Called by planner_recalculate() when the executing block is updated by the new plan. diff --git a/system.c b/system.c new file mode 100644 index 0000000..4dfe31d --- /dev/null +++ b/system.c @@ -0,0 +1,200 @@ +/* + system.c - Handles system level commands and real-time processes + Part of Grbl + + Copyright (c) 2014 Sungeun K. 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 + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#include "system.h" +#include "settings.h" +#include "gcode.h" +#include "motion_control.h" +#include "report.h" +#include "print.h" + + +void system_init() +{ + PINOUT_DDR &= ~(PINOUT_MASK); // Set as input pins + PINOUT_PORT |= PINOUT_MASK; // Enable internal pull-up resistors. Normal high operation. + PINOUT_PCMSK |= PINOUT_MASK; // Enable specific pins of the Pin Change Interrupt + PCICR |= (1 << PINOUT_INT); // Enable Pin Change Interrupt +} + + +// Pin change interrupt for pin-out commands, i.e. cycle start, feed hold, and reset. Sets +// only the runtime command execute variable to have the main program execute these when +// its ready. This works exactly like the character-based runtime commands when picked off +// directly from the incoming serial data stream. +ISR(PINOUT_INT_vect) +{ + // Enter only if any pinout pin is actively low. + if ((PINOUT_PIN & PINOUT_MASK) ^ PINOUT_MASK) { + if (bit_isfalse(PINOUT_PIN,bit(PIN_RESET))) { + mc_reset(); + } else if (bit_isfalse(PINOUT_PIN,bit(PIN_FEED_HOLD))) { + sys.execute |= EXEC_FEED_HOLD; + } else if (bit_isfalse(PINOUT_PIN,bit(PIN_CYCLE_START))) { + sys.execute |= EXEC_CYCLE_START; + } + } +} + + +// Executes user startup script, if stored. +void system_execute_startup(char *line) +{ + uint8_t n; + for (n=0; n < N_STARTUP_LINE; n++) { + if (!(settings_read_startup_line(n, line))) { + report_status_message(STATUS_SETTING_READ_FAIL); + } else { + if (line[0] != 0) { + printString(line); // Echo startup line to indicate execution. + report_status_message(gc_execute_line(line)); + } + } + } +} + + +// Directs and executes one line of formatted input from protocol_process. While mostly +// incoming streaming g-code blocks, this also executes Grbl internal commands, such as +// settings, initiating the homing cycle, and toggling switch states. This differs from +// the runtime command module by being susceptible to when Grbl is ready to execute the +// next line during a cycle, so for switches like block delete, the switch only effects +// the lines that are processed afterward, not necessarily real-time during a cycle, +// since there are motions already stored in the buffer. However, this 'lag' should not +// be an issue, since these commands are not typically used during a cycle. +uint8_t system_execute_line(char *line) +{ + uint8_t char_counter = 1; + uint8_t helper_var = 0; // Helper variable + float parameter, value; + switch( line[char_counter] ) { + case '#' : // Print gcode parameters + if ( line[++char_counter] != 0 ) { return(STATUS_UNSUPPORTED_STATEMENT); } + else { report_gcode_parameters(); } + break; + case 'G' : // Prints gcode parser state + if ( line[++char_counter] != 0 ) { return(STATUS_UNSUPPORTED_STATEMENT); } + else { report_gcode_modes(); } + break; +// case 'J' : break; // Jogging methods + // TODO: Here jogging can be placed for execution as a seperate subprogram. It does not need to be + // susceptible to other runtime commands except for e-stop. The jogging function is intended to + // be a basic toggle on/off with controlled acceleration and deceleration to prevent skipped + // steps. The user would supply the desired feedrate, axis to move, and direction. Toggle on would + // start motion and toggle off would initiate a deceleration to stop. One could 'feather' the + // motion by repeatedly toggling to slow the motion to the desired location. Location data would + // need to be updated real-time and supplied to the user through status queries. + // More controlled exact motions can be taken care of by inputting G0 or G1 commands, which are + // handled by the planner. It would be possible for the jog subprogram to insert blocks into the + // block buffer without having the planner plan them. It would need to manage de/ac-celerations + // on its own carefully. This approach could be effective and possibly size/memory efficient. + default : + // Block any system command that requires the state as IDLE/ALARM. (i.e. EEPROM, homing) + if ( !(sys.state == STATE_IDLE || sys.state == STATE_ALARM) ) { return(STATUS_IDLE_ERROR); } + switch( line[char_counter] ) { + case 0 : report_grbl_help(); break; + case '$' : // Prints Grbl settings + if ( line[++char_counter] != 0 ) { return(STATUS_UNSUPPORTED_STATEMENT); } + else { report_grbl_settings(); } + break; + case 'C' : // Set check g-code mode + if ( line[++char_counter] != 0 ) { return(STATUS_UNSUPPORTED_STATEMENT); } + // Perform reset when toggling off. Check g-code mode should only work if Grbl + // is idle and ready, regardless of alarm locks. This is mainly to keep things + // simple and consistent. + if ( sys.state == STATE_CHECK_MODE ) { + mc_reset(); + report_feedback_message(MESSAGE_DISABLED); + } else { + if (sys.state) { return(STATUS_IDLE_ERROR); } + sys.state = STATE_CHECK_MODE; + report_feedback_message(MESSAGE_ENABLED); + } + break; + case 'X' : // Disable alarm lock + if ( line[++char_counter] != 0 ) { return(STATUS_UNSUPPORTED_STATEMENT); } + if (sys.state == STATE_ALARM) { + report_feedback_message(MESSAGE_ALARM_UNLOCK); + sys.state = STATE_IDLE; + // Don't run startup script. Prevents stored moves in startup from causing accidents. + } + break; + case 'H' : // Perform homing cycle + if (bit_istrue(settings.flags,BITFLAG_HOMING_ENABLE)) { + // Only perform homing if Grbl is idle or lost. + mc_homing_cycle(); + if (!sys.abort) { system_execute_startup(line); } // Execute startup scripts after successful homing. + } else { return(STATUS_SETTING_DISABLED); } + break; + case 'I' : // Print or store build info. + if ( line[++char_counter] == 0 ) { + if (!(settings_read_build_info(line))) { + report_status_message(STATUS_SETTING_READ_FAIL); + } else { + report_build_info(line); + } + } else { // Store startup line + if(line[char_counter++] != '=') { return(STATUS_UNSUPPORTED_STATEMENT); } + helper_var = char_counter; // Set helper variable as counter to start of user info line. + do { + line[char_counter-helper_var] = line[char_counter]; + } while (line[char_counter++] != 0); + settings_store_build_info(line); + } + break; + case 'N' : // Startup lines. + if ( line[++char_counter] == 0 ) { // Print startup lines + for (helper_var=0; helper_var < N_STARTUP_LINE; helper_var++) { + if (!(settings_read_startup_line(helper_var, line))) { + report_status_message(STATUS_SETTING_READ_FAIL); + } else { + report_startup_line(helper_var,line); + } + } + break; + } else { // Store startup line + helper_var = true; // Set helper_var to flag storing method. + // No break. Continues into default: to read remaining command characters. + } + default : // Storing setting methods + if(!read_float(line, &char_counter, ¶meter)) { return(STATUS_BAD_NUMBER_FORMAT); } + if(line[char_counter++] != '=') { return(STATUS_UNSUPPORTED_STATEMENT); } + if (helper_var) { // Store startup line + // Prepare sending gcode block to gcode parser by shifting all characters + helper_var = char_counter; // Set helper variable as counter to start of gcode block + do { + line[char_counter-helper_var] = line[char_counter]; + } while (line[char_counter++] != 0); + // Execute gcode block to ensure block is valid. + helper_var = gc_execute_line(line); // Set helper_var to returned status code. + if (helper_var) { return(helper_var); } + else { + helper_var = trunc(parameter); // Set helper_var to int value of parameter + settings_store_startup_line(helper_var,line); + } + } else { // Store global setting. + if(!read_float(line, &char_counter, &value)) { return(STATUS_BAD_NUMBER_FORMAT); } + if(line[char_counter] != 0) { return(STATUS_UNSUPPORTED_STATEMENT); } + return(settings_store_global_setting(parameter, value)); + } + } + } + return(STATUS_OK); // If '$' command makes it to here, then everything's ok. +} diff --git a/system.h b/system.h new file mode 100644 index 0000000..78faa5c --- /dev/null +++ b/system.h @@ -0,0 +1,95 @@ +/* + system.h - Header for system level commands and real-time processes + Part of Grbl + + Copyright (c) 2014 Sungeun K. 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 + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#ifndef system_h +#define system_h + +// Define system header files and standard libraries used by Grbl +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Define Grbl configuration and shared header files +#include "config.h" +#include "defaults.h" +#include "cpu_map.h" +#include "nuts_bolts.h" + + +// Define system executor bit map. Used internally by runtime protocol as runtime command flags, +// which notifies the main program to execute the specified runtime command asynchronously. +// NOTE: The system executor uses an unsigned 8-bit volatile variable (8 flag limit.) The default +// flags are always false, so the runtime protocol only needs to check for a non-zero value to +// know when there is a runtime command to execute. +#define EXEC_STATUS_REPORT bit(0) // bitmask 00000001 +#define EXEC_CYCLE_START bit(1) // bitmask 00000010 +#define EXEC_CYCLE_STOP bit(2) // bitmask 00000100 +#define EXEC_FEED_HOLD bit(3) // bitmask 00001000 +#define EXEC_RESET bit(4) // bitmask 00010000 +#define EXEC_ALARM bit(5) // bitmask 00100000 +#define EXEC_CRIT_EVENT bit(6) // bitmask 01000000 +// #define bit(7) // bitmask 10000000 + +// Define system state bit map. The state variable primarily tracks the individual functions +// of Grbl to manage each without overlapping. It is also used as a messaging flag for +// critical events. +#define STATE_IDLE 0 // Must be zero. No flags. +#define STATE_QUEUED bit(0) // Indicates buffered blocks, awaiting cycle start. +#define STATE_CYCLE bit(1) // Cycle is running +#define STATE_HOLD bit(2) // Executing feed hold +#define STATE_HOMING bit(3) // Performing homing cycle +#define STATE_ALARM bit(4) // In alarm state. Locks out all g-code processes. Allows settings access. +#define STATE_CHECK_MODE bit(5) // G-code check mode. Locks out planner and motion only. +// #define STATE_JOG bit(6) // Jogging mode is unique like homing. + +// Define global system variables +typedef struct { + uint8_t abort; // System abort flag. Forces exit back to main loop for reset. + uint8_t state; // Tracks the current state of Grbl. + volatile uint8_t execute; // Global system runtime executor bitflag variable. See EXEC bitmasks. + uint8_t homing_axis_lock; + int32_t position[N_AXIS]; // Real-time machine (aka home) position vector in steps. + // NOTE: This may need to be a volatile variable, if problems arise. + uint8_t auto_start; // Planner auto-start flag. Toggled off during feed hold. Defaulted by settings. +} system_t; +extern system_t sys; + + +// Initialize the serial protocol +void system_init(); + +// Executes an internal system command, defined as a string starting with a '$' +uint8_t system_execute_line(char *line); + +// Checks and executes a runtime command at various stop points in main program +void system_execute_runtime(); + +// Execute the startup script lines stored in EEPROM upon initialization +void system_execute_startup(char *line); + +#endif From 6fdb35a7dad3e4ef027260aa30aef1fb0e55c554 Mon Sep 17 00:00:00 2001 From: Robert Grzesek Date: Thu, 6 Feb 2014 15:10:27 -0800 Subject: [PATCH 40/73] Initial line number reporting --- gcode.c | 14 ++++++++------ limits.c | 2 +- motion_control.c | 12 ++++++------ motion_control.h | 5 +++-- planner.c | 3 ++- planner.h | 5 +++-- report.c | 12 ++++++++++++ 7 files changed, 35 insertions(+), 18 deletions(-) diff --git a/gcode.c b/gcode.c index 4ba45ba..a9a0dae 100644 --- a/gcode.c +++ b/gcode.c @@ -106,6 +106,7 @@ uint8_t gc_execute_line(char *line) float target[N_AXIS]; clear_vector(target); // XYZ(ABC) axes parameters. + uint32_t line_number = 0; gc.arc_radius = 0; clear_vector(gc.arc_offset); // IJK Arc offsets are incremental. Value of zero indicates no change. @@ -220,7 +221,7 @@ uint8_t gc_execute_line(char *line) char_counter = 0; while(next_statement(&letter, &value, line, &char_counter)) { switch(letter) { - case 'G': case 'M': case 'N': break; // Ignore command statements and line numbers + case 'G': case 'M': break; // Ignore command statements and line numbers case 'F': if (value <= 0) { FAIL(STATUS_INVALID_STATEMENT); } // Must be greater than zero if (gc.inverse_feed_rate_mode) { @@ -231,6 +232,7 @@ uint8_t gc_execute_line(char *line) break; case 'I': case 'J': case 'K': gc.arc_offset[letter-'I'] = to_millimeters(value); break; case 'L': l = trunc(value); break; + case 'N': line_number = trunc(value); break; case 'P': p = value; break; case 'R': gc.arc_radius = to_millimeters(value); break; case 'S': @@ -330,7 +332,7 @@ uint8_t gc_execute_line(char *line) target[idx] = gc.position[idx]; } } - mc_line(target, -1.0, false); + mc_line(target, -1.0, false, line_number); } // Retreive G28/30 go-home position data (in machine coordinates) from EEPROM float coord_data[N_AXIS]; @@ -339,7 +341,7 @@ uint8_t gc_execute_line(char *line) } else { if (!settings_read_coord_data(SETTING_INDEX_G30,coord_data)) { return(STATUS_SETTING_READ_FAIL); } } - mc_line(coord_data, -1.0, false); + mc_line(coord_data, -1.0, false, line_number); memcpy(gc.position, coord_data, sizeof(coord_data)); // gc.position[] = coord_data[]; axis_words = 0; // Axis words used. Lock out from motion modes by clearing flags. break; @@ -410,7 +412,7 @@ uint8_t gc_execute_line(char *line) break; case MOTION_MODE_SEEK: if (!axis_words) { FAIL(STATUS_INVALID_STATEMENT);} - else { mc_line(target, -1.0, false); } + else { mc_line(target, -1.0, false, line_number); } break; case MOTION_MODE_LINEAR: // TODO: Inverse time requires F-word with each statement. Need to do a check. Also need @@ -418,7 +420,7 @@ uint8_t gc_execute_line(char *line) // and after an inverse time move and then check for non-zero feed rate each time. This // should be efficient and effective. if (!axis_words) { FAIL(STATUS_INVALID_STATEMENT);} - else { mc_line(target, (gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode); } + else { mc_line(target, (gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode, line_number); } break; case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC: // Check if at least one of the axes of the selected plane has been specified. If in center @@ -442,7 +444,7 @@ uint8_t gc_execute_line(char *line) // Trace the arc mc_arc(gc.position, target, gc.arc_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.arc_radius, isclockwise); + gc.arc_radius, isclockwise, line_number); } break; } diff --git a/limits.c b/limits.c index 169ca71..486f389 100644 --- a/limits.c +++ b/limits.c @@ -157,7 +157,7 @@ void limits_go_home(uint8_t cycle_mask, bool approach, float homing_rate) // Perform homing cycle. Planner buffer should be empty, as required to initiate the homing cycle. uint8_t limit_state; - plan_buffer_line(target, homing_rate, false); // Bypass mc_line(). Directly plan homing motion. + plan_buffer_line(target, homing_rate, false, HOMING_CYCLE_LINE_NUMBER); // Bypass mc_line(). Directly plan homing motion. st_prep_buffer(); // Prep first segment from newly planned block. st_wake_up(); // Initiate motion do { diff --git a/motion_control.c b/motion_control.c index ee1798b..e336bfd 100644 --- a/motion_control.c +++ b/motion_control.c @@ -39,7 +39,7 @@ // segments, must pass through this routine before being passed to the planner. The seperation of // mc_line and plan_buffer_line is done primarily to place non-planner-type functions from being // in the planner and to let backlash compensation or canned cycle integration simple and direct. -void mc_line(float *target, float feed_rate, uint8_t invert_feed_rate) +void mc_line(float *target, float feed_rate, uint8_t invert_feed_rate, uint32_t line_number) { // If enabled, check for soft limit violations. Placed here all line motions are picked up // from everywhere in Grbl. @@ -68,7 +68,7 @@ void mc_line(float *target, float feed_rate, uint8_t invert_feed_rate) else { break; } } while (1); - plan_buffer_line(target, feed_rate, invert_feed_rate); + plan_buffer_line(target, feed_rate, invert_feed_rate, line_number); // If idle, indicate to the system there is now a planned block in the buffer ready to cycle // start. Otherwise ignore and continue on. @@ -84,7 +84,7 @@ void mc_line(float *target, float feed_rate, uint8_t invert_feed_rate) // 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) + uint8_t axis_linear, float feed_rate, uint8_t invert_feed_rate, float radius, uint8_t isclockwise, uint32_t line_number) { float center_axis0 = position[axis_0] + offset[axis_0]; float center_axis1 = position[axis_1] + offset[axis_1]; @@ -180,14 +180,14 @@ void mc_arc(float *position, float *target, float *offset, uint8_t axis_0, uint8 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, feed_rate, invert_feed_rate); + mc_line(arc_target, feed_rate, invert_feed_rate, line_number); // 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, feed_rate, invert_feed_rate); + mc_line(target, feed_rate, invert_feed_rate, line_number); } @@ -273,7 +273,7 @@ void mc_homing_cycle() sys.state = STATE_IDLE; // Set system state to IDLE to complete motion and indicate homed. - mc_line(pulloff_target, settings.homing_seek_rate, false); + mc_line(pulloff_target, settings.homing_seek_rate, false, HOMING_CYCLE_LINE_NUMBER); st_cycle_start(); // Move it. Nothing should be in the buffer except this motion. plan_synchronize(); // Make sure the motion completes. // NOTE: Stepper idle lock resumes normal functionality after cycle. diff --git a/motion_control.h b/motion_control.h index b17e168..0fe2960 100644 --- a/motion_control.h +++ b/motion_control.h @@ -22,18 +22,19 @@ #ifndef motion_control_h #define motion_control_h +#define HOMING_CYCLE_LINE_NUMBER 1000000000 // Execute linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second // unless invert_feed_rate is true. Then the feed_rate means that the motion should be completed in // (1 minute)/feed_rate time. -void mc_line(float *target, float feed_rate, uint8_t invert_feed_rate); +void mc_line(float *target, float feed_rate, uint8_t invert_feed_rate, uint32_t line_number); // 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, isclockwise boolean. Used // for vector transformation direction. 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); + uint8_t axis_linear, float feed_rate, uint8_t invert_feed_rate, float radius, uint8_t isclockwise, uint32_t line_number); // Dwell for a specific number of seconds void mc_dwell(float seconds); diff --git a/planner.c b/planner.c index 9e435c7..f167522 100644 --- a/planner.c +++ b/planner.c @@ -272,7 +272,7 @@ void plan_synchronize() is used in three ways: as a normal feed rate if invert_feed_rate is false, as inverse time if invert_feed_rate is true, or as seek/rapids rate if the feed_rate value is negative (and invert_feed_rate always false). */ -void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) +void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate, uint32_t line_number) { // Prepare and initialize new block plan_block_t *block = &block_buffer[block_buffer_head]; @@ -280,6 +280,7 @@ void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) block->millimeters = 0; block->direction_bits = 0; block->acceleration = SOME_LARGE_VALUE; // Scaled down to maximum acceleration later + block->line_number = line_number; // Compute and store initial move distance data. // TODO: After this for-loop, we don't touch the stepper algorithm data. Might be a good idea diff --git a/planner.h b/planner.h index 99298ed..e7ae716 100644 --- a/planner.h +++ b/planner.h @@ -26,7 +26,7 @@ // The number of linear motions that can be in the plan at any give time #ifndef BLOCK_BUFFER_SIZE - #define BLOCK_BUFFER_SIZE 18 + #define BLOCK_BUFFER_SIZE 16 #endif // This struct stores a linear movement of a g-code block motion with its critical "nominal" values @@ -47,6 +47,7 @@ typedef struct { float acceleration; // Axis-limit adjusted line acceleration in (mm/min^2) float millimeters; // The remaining distance for this block to be executed in (mm) // uint8_t max_override; // Maximum override value based on axis speed limits + uint32_t line_number; } plan_block_t; @@ -56,7 +57,7 @@ void plan_reset(); // Add a new linear movement to the buffer. target[N_AXIS] is the signed, absolute target position // in millimeters. 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(float *target, float feed_rate, uint8_t invert_feed_rate); +void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate, uint32_t line_number); // Called when the current block is no longer needed. Discards the block and makes the memory // availible for new blocks. diff --git a/report.c b/report.c index ee4de61..d62ed49 100644 --- a/report.c +++ b/report.c @@ -32,6 +32,7 @@ #include "settings.h" #include "gcode.h" #include "coolant_control.h" +#include "planner.h" // Handles the primary confirmation protocol response for streaming interfaces and human-feedback. @@ -348,6 +349,17 @@ void report_realtime_status() printFloat(print_position[i]); if (i < (N_AXIS-1)) { printPgmString(PSTR(",")); } } + + // Report current line number + printPgmString(PSTR(",")); + printPgmString(PSTR("Ln:")); + uint32_t ln=0; + plan_block_t * pb = plan_get_current_block(); + if(pb != NULL) { + ln = pb->line_number; + } + printInteger(ln); + printPgmString(PSTR(">\r\n")); } From cd71a90ce8a770e0030ed6c9bac805b89724e275 Mon Sep 17 00:00:00 2001 From: Robert Grzesek Date: Thu, 6 Feb 2014 15:19:30 -0800 Subject: [PATCH 41/73] Made line number reporting optional via config.h --- config.h | 4 ++++ planner.c | 3 ++- planner.h | 8 +++++++- report.c | 3 ++- 4 files changed, 15 insertions(+), 3 deletions(-) diff --git a/config.h b/config.h index dbf37e3..ddaddcb 100644 --- a/config.h +++ b/config.h @@ -28,6 +28,10 @@ #ifndef config_h #define config_h +// Allows GRBL to tranck and report gcode line numbers. Enabling this means that the planning buffer +// goes from 18 or 16 to make room for the additional line number data in the plan_block_t struct +#define USE_LINE_NUMBERS + // Default settings. Used when resetting EEPROM. Change to desired name in defaults.h #define DEFAULTS_SHERLINE_5400 diff --git a/planner.c b/planner.c index f167522..dfc56db 100644 --- a/planner.c +++ b/planner.c @@ -280,8 +280,9 @@ void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate, block->millimeters = 0; block->direction_bits = 0; block->acceleration = SOME_LARGE_VALUE; // Scaled down to maximum acceleration later +#ifdef USE_LINE_NUMBERS block->line_number = line_number; - +#endif // Compute and store initial move distance data. // TODO: After this for-loop, we don't touch the stepper algorithm data. Might be a good idea // to try to keep these types of things completely separate from the planner for portability. diff --git a/planner.h b/planner.h index e7ae716..7a255be 100644 --- a/planner.h +++ b/planner.h @@ -26,7 +26,11 @@ // The number of linear motions that can be in the plan at any give time #ifndef BLOCK_BUFFER_SIZE - #define BLOCK_BUFFER_SIZE 16 + #ifdef USE_LINE_NUMBERS + #define BLOCK_BUFFER_SIZE 16 + #else + #define BLOCK_BUFFER_SIZE 18 + #endif #endif // This struct stores a linear movement of a g-code block motion with its critical "nominal" values @@ -47,7 +51,9 @@ typedef struct { float acceleration; // Axis-limit adjusted line acceleration in (mm/min^2) float millimeters; // The remaining distance for this block to be executed in (mm) // uint8_t max_override; // Maximum override value based on axis speed limits +#ifdef USE_LINE_NUMBERS uint32_t line_number; +#endif } plan_block_t; diff --git a/report.c b/report.c index d62ed49..5b97dc1 100644 --- a/report.c +++ b/report.c @@ -350,6 +350,7 @@ void report_realtime_status() if (i < (N_AXIS-1)) { printPgmString(PSTR(",")); } } +#ifdef USE_LINE_NUMBERS // Report current line number printPgmString(PSTR(",")); printPgmString(PSTR("Ln:")); @@ -359,7 +360,7 @@ void report_realtime_status() ln = pb->line_number; } printInteger(ln); - +#endif printPgmString(PSTR(">\r\n")); } From 50fbc6e2972acadbe5db944ddf33d12a8d94f4a0 Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Sun, 9 Feb 2014 10:46:34 -0700 Subject: [PATCH 42/73] Refactoring and lots of bug fixes. Updated homing cycle. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit WARNING: There are still some bugs to be worked out. Please use caution if you test this firmware. - Feed holds work much better, but there are still some failure conditions that need to be worked out. This is the being worked on currently and a fix is planned to be pushed next. - Homing cycle refactoring: Slight adjustment of the homing cycle to allow for limit pins to be shared by different axes, as long as the shared limit pins are not homed on the same cycle. Also, removed the LOCATE_CYCLE portion of the homing cycle configuration. It was redundant. - Limit pin sharing: (See above). To clear up one or two limit pins for other IO, limit pins can now be shared. For example, the Z-limit can be shared with either X or Y limit pins, because it’s on a separate homing cycle. Hard limit will still work exactly as before. - Spindle pin output fixed. The pins weren’t getting initialized correctly. - Fixed a cycle issue where streaming was working almost like a single block mode. This was caused by a problem with the spindle_run() and coolant_run() commands and issuing an unintended planner buffer sync. - Refactored the cycle_start, feed_hold, and other runtime routines into the runtime command module, where they should be handled here only. These were redundant. - Moved some function calls around into more appropriate source code modules. - Fixed the reporting of spindle state. --- README.md | 15 ++- config.h | 34 +++--- coolant_control.c | 4 +- cpu_map.h | 10 +- gcode.c | 23 ++-- gcode.h | 2 +- limits.c | 228 ++++++++++++++++++++++-------------- limits.h | 2 +- main.c | 14 ++- motion_control.c | 90 +++----------- motion_control.h | 3 - planner.c | 16 +-- planner.h | 3 - protocol.c | 251 +++++++++++++++++++++++---------------- protocol.h | 23 +++- report.c | 7 +- spindle_control.c | 22 ++-- stepper.c | 291 +++++++++++++++++++++++----------------------- stepper.h | 9 -- system.c | 61 +++++----- 20 files changed, 579 insertions(+), 529 deletions(-) diff --git a/README.md b/README.md index c58f3a1..119ef70 100644 --- a/README.md +++ b/README.md @@ -15,17 +15,20 @@ Grbl includes full acceleration management with look ahead. That means the contr ##Changelog for v0.9 from v0.8 - **ALPHA status: Under heavy development.** - - New stepper algorithm: Based on an inverse time algorithm, but modified to ensure steps are executed exactly. This algorithm performs a constant timer tick and has a hard limit of 30kHz maximum step frequency. It is also highly tuneable and should be very easy to port to other microcontroller architectures. Overall, a much better, smoother stepper algorithm with the capability of very high speeds. - - Planner optimizations: Planning computations improved four-fold or more. Changes include streaming optimizations by ignoring already optimized blocks and removing redundant variables and computations and offloading them to the stepper algorithm on an ad-hoc basis. + - New stepper algorithm: Complete overhaul of the handling of the stepper driver to simplify and reduce task time per ISR tick. Much smoother operation with the new Adaptive Multi-Axis Step Smoothing (AMASS) algorithm which does what its name implies. Users should audibly hear significant differences in how their machines move and see improved overall performance! + - Planner optimizations: Planning computations improved four-fold or more by optimizing end-to-end operations. Changes include streaming optimizations by ignoring already optimized blocks and removing redundant variables and computations and offloading them to the stepper algorithm to compute on an ad-hoc basis. - Planner stability: Previous Grbl planners have all had a corruption issue in rare circumstances that becomes particularly problematic at high step frequencies and when jogging. The new planner is robust and incorruptible, meaning that we can fearlessly drive Grbl to it's highest limits. Combined with the new stepper algorithm and planner optimizations, this means 5x to 10x performance increases in our testing! This is all achieved through the introduction of an intermediary step segment buffer that "checks-out" steps from the planner buffer in real-time. - Acceleration independence: Each axes may be defined with different acceleration parameters and Grbl will automagically calculate the maximum acceleration through a path depending on the direction traveled. This is very useful for machine that have very different axes properties, like the ShapeOko z-axis. - Maximum velocity independence: As with acceleration, the maximum velocity of individual axes may be defined. All seek/rapids motions will move at these maximum rates, but never exceed any one axes. So, when two or more axes move, the limiting axis will move at its maximum rate, while the other axes are scaled down. - - Significantly improved arc performance: Arcs are now defined in terms of chordal tolerance, rather than segment length. Chordal tolerance will automatically scale all arc line segments depending on arc radius, such that the error does not exceed the tolerance value (default: 0.005 mm.) So, for larger radii arcs, Grbl can move faster through them, because the segments are always longer and the planner has more distance to plan with. + - Significantly improved arc performance: Arcs are now defined in terms of chordal tolerance, rather than segment length. Chordal tolerance will automatically scale all arc line segments depending on arc radius, such that the error does not exceed the tolerance value (default: 0.002 mm.) So, for larger radii arcs, Grbl can move faster through them, because the segments are always longer and the planner has more distance to plan with. - Soft limits: Checks if any motion command exceeds workspace limits. Alarms out when found. Another safety feature, but, unlike hard limits, position does not get lost, as it forces a feed hold before erroring out. - - Pin mapping: In an effort for Grbl to be compatible with other AVR architectures, such as the 1280 or 2560, a new pin_map.h configuration file has been created to allow Grbl to be compiled for them. This is currently user supported, so your mileage may vary. If you run across a bug, please let us know or better send us a fix! Thanks in advance! + - CPU pin mapping: In an effort for Grbl to be compatible with other AVR architectures, such as the 1280 or 2560, a new cpu_map.h pin configuration file has been created to allow Grbl to be compiled for them. This is currently user supported, so your mileage may vary. If you run across a bug, please let us know or better send us a fix! Thanks in advance! - New Grbl SIMULATOR by @jgeisler: A completely independent wrapper of the Grbl main source code that may be compiled as an executable on a computer. No Arduino required. Simply simulates the responses of Grbl as if it was on an Arduino. May be used for many things: checking out how Grbl works, pre-process moves for GUI graphics, debugging of new features, etc. Much left to do, but potentially very powerful, as the dummy AVR variables can be written to output anything you need. - Homing routine updated: Sets workspace volume in all negative space regardless of limit switch position. Common on pro CNCs. Now tied directly into the main planner and stepper modules to reduce flash space and allow maximum speeds during seeking. - - Feedrate overrides: In the works, but planner has begun to be re-factored for this feature. - - Jogging controls: Methodology needs to be to figured out first. Could be dropped due to flash space concerns. Last item on the agenda. + - Combined limit pins capability: Limit switches can be combined to share the same pins to free up precious I/O pins for other purposes. When combined, users must adjust the homing cycle mask in config.h to not home the axes on a shared pin at the same time. + - Variable spindle speed output: Available only as a compile-time option through the config.h file. Enables PWM output for 'S' g-code commands. Enabling this feature will swap the Z-limit D11 pin and spindle enable D12 pin to access the hardware PWM on pin D12. The Z-limit pin, now on D12, should work just as it did before. + - Increased serial baud rate: Default serial baudrate is now 115200, because the new planner and stepper allows much higher processing and execution speeds that 9600 baud was just not cutting it. + - Feedrate overrides: (Slated for v1.0 release) The framework to enable feedrate overrides is in-place with v0.9, but the minor details has not yet been installed. + - Jogging controls: (Slated for v1.0 release) Methodology needs to be to figured out first. Could be dropped due to flash space concerns. _The project was initially inspired by the Arduino GCode Interpreter by Mike Ellery_ diff --git a/config.h b/config.h index dbf37e3..84eff6f 100644 --- a/config.h +++ b/config.h @@ -54,19 +54,24 @@ // mainly a safety feature to remind the user to home, since position is unknown to Grbl. #define HOMING_INIT_LOCK // Comment to disable -// Define the homing cycle search patterns with bitmasks. The homing cycle first performs a search -// to engage the limit switches. HOMING_SEARCH_CYCLE_x are executed in order starting with suffix 0 -// and searches the enabled axes in the bitmask. This allows for users with non-standard cartesian -// machines, such as a lathe (x then z), to configure the homing cycle behavior to their needs. -// Search cycle 0 is required, but cycles 1 and 2 are both optional and may be commented to disable. -// After the search cycle, homing then performs a series of locating about the limit switches to hone -// in on machine zero, followed by a pull-off maneuver. HOMING_LOCATE_CYCLE governs these final moves, -// and this mask must contain all axes in the search. -// NOTE: Later versions may have this installed in settings. -#define HOMING_SEARCH_CYCLE_0 (1< 1. + + void limits_init() { LIMIT_DDR &= ~(LIMIT_MASK); // Set as input pins @@ -65,24 +68,38 @@ void limits_disable() // limit switch can cause a lot of problems, like false readings and multiple interrupt calls. // If a switch is triggered at all, something bad has happened and treat it as such, regardless // if a limit switch is being disengaged. It's impossible to reliably tell the state of a -// bouncing pin without a debouncing method. +// bouncing pin without a debouncing method. A simple software debouncing feature may be enabled +// through the config.h file, where an extra timer delays the limit pin read by several milli- +// seconds to help with, not fix, bouncing switches. // NOTE: Do not attach an e-stop to the limit pins, because this interrupt is disabled during // homing cycles and will not respond correctly. Upon user request or need, there may be a // special pinout for an e-stop, but it is generally recommended to just directly connect // your e-stop switch to the Arduino reset pin, since it is the most correct way to do this. -#ifdef ENABLE_SOFTWARE_DEBOUNCE - ISR(LIMIT_INT_vect) { if (!(WDTCSR & (1< settings.max_travel[Y_AXIS]) { max_travel = settings.max_travel[Y_AXIS]; } if (max_travel > settings.max_travel[Z_AXIS]) { max_travel = settings.max_travel[Z_AXIS]; } - max_travel *= -1.25; // Ensure homing switches engaged by over-estimating max travel. - if (!approach) { max_travel = -max_travel; } - - // Set target location and rate for active axes. - float target[N_AXIS]; - uint8_t n_active_axis = 0; - uint8_t i; - for (i=0; i 0); + + // The active cycle axes should now be homed and machine limits have been located. By + // default, grbl defines machine space as all negative, as do most CNCs. Since limit switches + // can be on either side of an axes, check and set axes machine zero appropriately. Also, + // set up pull-off maneuver from axes limit switches that have been homed. This provides + // some initial clearance off the switches and should also help prevent them from falsely + // triggering when hard limits are enabled or when more than one axes shares a limit pin. + for (idx=0; idx 0) { // NOTE: Check and execute runtime commands during dwell every <= DWELL_TIME_STEP milliseconds. protocol_execute_runtime(); @@ -213,93 +213,37 @@ void mc_homing_cycle() { sys.state = STATE_HOMING; // Set system state variable limits_disable(); // Disable hard limits pin change register for cycle duration - plan_reset(); // Reset planner buffer before beginning homing routine. - st_reset(); // Reset step segment buffer before beginning homing routine. // ------------------------------------------------------------------------------------- // Perform homing routine. NOTE: Special motion case. Only system reset works. // Search to engage all axes limit switches at faster homing seek rate. - limits_go_home(HOMING_SEARCH_CYCLE_0, true, settings.homing_seek_rate); // Search cycle 0 - #ifdef HOMING_SEARCH_CYCLE_1 - limits_go_home(HOMING_SEARCH_CYCLE_1, true, settings.homing_seek_rate); // Search cycle 1 + limits_go_home(HOMING_CYCLE_0); // Homing cycle 0 + #ifdef HOMING_CYCLE_1 + limits_go_home(HOMING_CYCLE_1); // Homing cycle 1 #endif - #ifdef HOMING_SEARCH_CYCLE_2 - limits_go_home(HOMING_SEARCH_CYCLE_2, true, settings.homing_seek_rate); // Search cycle 2 + #ifdef HOMING_CYCLE_2 + limits_go_home(HOMING_CYCLE_2); // Homing cycle 2 #endif - - // Now in proximity of all limits. Carefully leave and approach switches in multiple cycles - // to precisely hone in on the machine zero location. Moves at slower homing feed rate. - int8_t n_cycle = N_HOMING_LOCATE_CYCLE; - while (n_cycle--) { - // Leave all switches to release them. After cycles complete, this is machine zero. - limits_go_home(HOMING_LOCATE_CYCLE, false, settings.homing_feed_rate); - - if (n_cycle > 0) { - // Re-approach all switches to re-engage them. - limits_go_home(HOMING_LOCATE_CYCLE, true, settings.homing_feed_rate); - } - } - // ------------------------------------------------------------------------------------- protocol_execute_runtime(); // Check for reset and set system abort. if (sys.abort) { return; } // Did not complete. Alarm state set by mc_alarm. - // The machine should now be homed and machine limits have been located. By default, - // grbl defines machine space as all negative, as do most CNCs. Since limit switches - // can be on either side of an axes, check and set machine zero appropriately. - // At the same time, set up pull-off maneuver from axes limit switches that have been homed. - // This provides some initial clearance off the switches and should also help prevent them - // from falsely tripping when hard limits are enabled. - float pulloff_target[N_AXIS]; - clear_vector_float(pulloff_target); // Zero pulloff target. - clear_vector_long(sys.position); // Zero current position for now. - uint8_t idx; - for (idx=0; idxmax_junction_speed_sqr = max( MINIMUM_JUNCTION_SPEED*MINIMUM_JUNCTION_SPEED, (block->acceleration * settings.junction_deviation * sin_theta_d2)/(1.0-sin_theta_d2) ); } diff --git a/planner.h b/planner.h index 99298ed..535291e 100644 --- a/planner.h +++ b/planner.h @@ -80,7 +80,4 @@ void plan_cycle_reinitialize(); // Returns the status of the block ring buffer. True, if buffer is full. uint8_t plan_check_full_buffer(); -// Block until all buffered steps are executed -void plan_synchronize(); - #endif diff --git a/protocol.c b/protocol.c index ade1df0..7c7ae2c 100644 --- a/protocol.c +++ b/protocol.c @@ -1,5 +1,5 @@ /* - protocol.c - controls Grbl execution procedures + protocol.c - controls Grbl execution protocol and procedures Part of Grbl Copyright (c) 2011-2014 Sungeun K. Jeon @@ -24,6 +24,7 @@ #include "settings.h" #include "protocol.h" #include "gcode.h" +#include "planner.h" #include "stepper.h" #include "motion_control.h" #include "report.h" @@ -32,95 +33,6 @@ static char line[LINE_BUFFER_SIZE]; // Line to be executed. Zero-terminated. -// Executes run-time commands, when required. This is called from various check points in the main -// program, primarily where there may be a while loop waiting for a buffer to clear space or any -// point where the execution time from the last check point may be more than a fraction of a second. -// This is a way to execute runtime commands asynchronously (aka multitasking) with grbl's g-code -// parsing and planning functions. This function also serves as an interface for the interrupts to -// set the system runtime flags, where only the main program handles them, removing the need to -// define more computationally-expensive volatile variables. This also provides a controlled way to -// execute certain tasks without having two or more instances of the same task, such as the planner -// recalculating the buffer upon a feedhold or override. -// NOTE: The sys.execute variable flags are set by any process, step or serial interrupts, pinouts, -// limit switches, or the main program. -void protocol_execute_runtime() -{ - // Reload step segment buffer - st_prep_buffer(); - - if (sys.execute) { // Enter only if any bit flag is true - uint8_t rt_exec = sys.execute; // Avoid calling volatile multiple times - - // System alarm. Everything has shutdown by something that has gone severely wrong. Report - // the source of the error to the user. If critical, Grbl disables by entering an infinite - // loop until system reset/abort. - if (rt_exec & (EXEC_ALARM | EXEC_CRIT_EVENT)) { - sys.state = STATE_ALARM; // Set system alarm state - - // Critical event. Only hard/soft limit errors currently qualify. - if (rt_exec & EXEC_CRIT_EVENT) { - report_alarm_message(ALARM_LIMIT_ERROR); - report_feedback_message(MESSAGE_CRITICAL_EVENT); - bit_false(sys.execute,EXEC_RESET); // Disable any existing reset - do { - // Nothing. Block EVERYTHING until user issues reset or power cycles. Hard limits - // typically occur while unattended or not paying attention. Gives the user time - // to do what is needed before resetting, like killing the incoming stream. The - // same could be said about soft limits. While the position is not lost, the incoming - // stream could be still engaged and cause a serious crash if it continues afterwards. - } while (bit_isfalse(sys.execute,EXEC_RESET)); - - // Standard alarm event. Only abort during motion qualifies. - } else { - // Runtime abort command issued during a cycle, feed hold, or homing cycle. Message the - // user that position may have been lost and set alarm state to enable the alarm lockout - // to indicate the possible severity of the problem. - report_alarm_message(ALARM_ABORT_CYCLE); - } - bit_false(sys.execute,(EXEC_ALARM | EXEC_CRIT_EVENT)); - } - - // Execute system abort. - if (rt_exec & EXEC_RESET) { - sys.abort = true; // Only place this is set true. - return; // Nothing else to do but exit. - } - - // Execute and serial print status - if (rt_exec & EXEC_STATUS_REPORT) { - report_realtime_status(); - bit_false(sys.execute,EXEC_STATUS_REPORT); - } - - // Initiate stepper feed hold - if (rt_exec & EXEC_FEED_HOLD) { - // !!! During a cycle, the segment buffer has just been reloaded and full. So the math involved - // with the feed hold should be fine for most, if not all, operational scenarios. - st_feed_hold(); // Initiate feed hold. - bit_false(sys.execute,EXEC_FEED_HOLD); - } - - // Reinitializes the stepper module running state and, if a feed hold, re-plans the buffer. - // NOTE: EXEC_CYCLE_STOP is set by the stepper subsystem when a cycle or feed hold completes. - if (rt_exec & EXEC_CYCLE_STOP) { - st_cycle_reinitialize(); - bit_false(sys.execute,EXEC_CYCLE_STOP); - } - - if (rt_exec & EXEC_CYCLE_START) { - st_cycle_start(); // Issue cycle start command to stepper subsystem - if (bit_istrue(settings.flags,BITFLAG_AUTO_START)) { - sys.auto_start = true; // Re-enable auto start after feed hold. - } - bit_false(sys.execute,EXEC_CYCLE_START); - } - } - - // Overrides flag byte (sys.override) and execution should be installed here, since they - // are runtime and require a direct and controlled interface to the main stepper program. -} - - // Directs and executes one line of formatted input from protocol_process. While mostly // incoming streaming g-code blocks, this also directs and executes Grbl internal commands, // such as settings, initiating the homing cycle, and toggling switch states. @@ -141,20 +53,27 @@ static void protocol_execute_line(char *line) status = system_execute_line(line); } else { - // Everything else is gcode. Send to g-code parser! + // Everything else is gcode. Send to g-code parser! Block if in alarm mode. + if (sys.state == STATE_ALARM) { status = STATUS_ALARM_LOCK; } + else { status = gc_execute_line(line); } + // TODO: Separate the parsing from the g-code execution. Need to re-write the parser // completely to do this. First parse the line completely, checking for modal group // errors and storing all of the g-code words. Then, send the stored g-code words to // a separate g-code executor. This will be more in-line with actual g-code protocol. - status = gc_execute_line(line); - + + // TODO: Clean up the multi-tasking workflow with the execution of commands. It's a + // bit complicated and patch-worked. Could be made simplier to understand. } report_status_message(status); } -void protocol_process() +/* + GRBL MAIN LOOP: +*/ +void protocol_main_loop() { // ------------------------------------------------------------ // Complete initialization procedures upon a power-up or reset. @@ -218,15 +137,149 @@ void protocol_process() } } - protocol_execute_runtime(); // Runtime command check point. - if (sys.abort) { return; } // Bail to main() program loop to reset system. - // If there are no more characters in the serial read buffer to be processed and executed, // this indicates that g-code streaming has either filled the planner buffer or has // completed. In either case, auto-cycle start, if enabled, any queued moves. - mc_auto_cycle_start(); - + protocol_auto_cycle_start(); + + protocol_execute_runtime(); // Runtime command check point. + if (sys.abort) { return; } // Bail to main() program loop to reset system. + } return; /* Never reached */ } + + +// Executes run-time commands, when required. This is called from various check points in the main +// program, primarily where there may be a while loop waiting for a buffer to clear space or any +// point where the execution time from the last check point may be more than a fraction of a second. +// This is a way to execute runtime commands asynchronously (aka multitasking) with grbl's g-code +// parsing and planning functions. This function also serves as an interface for the interrupts to +// set the system runtime flags, where only the main program handles them, removing the need to +// define more computationally-expensive volatile variables. This also provides a controlled way to +// execute certain tasks without having two or more instances of the same task, such as the planner +// recalculating the buffer upon a feedhold or override. +// NOTE: The sys.execute variable flags are set by any process, step or serial interrupts, pinouts, +// limit switches, or the main program. +void protocol_execute_runtime() +{ + uint8_t rt_exec = sys.execute; // Copy to avoid calling volatile multiple times + if (rt_exec) { // Enter only if any bit flag is true + + // System alarm. Everything has shutdown by something that has gone severely wrong. Report + // the source of the error to the user. If critical, Grbl disables by entering an infinite + // loop until system reset/abort. + if (rt_exec & (EXEC_ALARM | EXEC_CRIT_EVENT)) { + sys.state = STATE_ALARM; // Set system alarm state + + // Critical event. Only hard/soft limit errors currently qualify. + if (rt_exec & EXEC_CRIT_EVENT) { + report_alarm_message(ALARM_LIMIT_ERROR); + report_feedback_message(MESSAGE_CRITICAL_EVENT); + bit_false(sys.execute,EXEC_RESET); // Disable any existing reset + do { + // Nothing. Block EVERYTHING until user issues reset or power cycles. Hard limits + // typically occur while unattended or not paying attention. Gives the user time + // to do what is needed before resetting, like killing the incoming stream. The + // same could be said about soft limits. While the position is not lost, the incoming + // stream could be still engaged and cause a serious crash if it continues afterwards. + } while (bit_isfalse(sys.execute,EXEC_RESET)); + + // Standard alarm event. Only abort during motion qualifies. + } else { + // Runtime abort command issued during a cycle, feed hold, or homing cycle. Message the + // user that position may have been lost and set alarm state to enable the alarm lockout + // to indicate the possible severity of the problem. + report_alarm_message(ALARM_ABORT_CYCLE); + } + bit_false(sys.execute,(EXEC_ALARM | EXEC_CRIT_EVENT)); + } + + // Execute system abort. + if (rt_exec & EXEC_RESET) { + sys.abort = true; // Only place this is set true. + return; // Nothing else to do but exit. + } + + // Execute and serial print status + if (rt_exec & EXEC_STATUS_REPORT) { + report_realtime_status(); + bit_false(sys.execute,EXEC_STATUS_REPORT); + } + + // Execute a feed hold with deceleration, only during cycle. + if (rt_exec & EXEC_FEED_HOLD) { + // !!! During a cycle, the segment buffer has just been reloaded and full. So the math involved + // with the feed hold should be fine for most, if not all, operational scenarios. + if (sys.state == STATE_CYCLE) { + sys.state = STATE_HOLD; + st_update_plan_block_parameters(); + st_prep_buffer(); + sys.auto_start = false; // Disable planner auto start upon feed hold. + } + bit_false(sys.execute,EXEC_FEED_HOLD); + } + + // Execute a cycle start by starting the stepper interrupt begin executing the blocks in queue. + if (rt_exec & EXEC_CYCLE_START) { + if (sys.state == STATE_QUEUED) { + sys.state = STATE_CYCLE; + st_prep_buffer(); // Initialize step segment buffer before beginning cycle. + st_wake_up(); + if (bit_istrue(settings.flags,BITFLAG_AUTO_START)) { + sys.auto_start = true; // Re-enable auto start after feed hold. + } else { + sys.auto_start = false; // Reset auto start per settings. + } + } + bit_false(sys.execute,EXEC_CYCLE_START); + } + + // Reinitializes the cycle plan and stepper system after a feed hold for a resume. Called by + // runtime command execution in the main program, ensuring that the planner re-plans safely. + // NOTE: Bresenham algorithm variables are still maintained through both the planner and stepper + // cycle reinitializations. The stepper path should continue exactly as if nothing has happened. + // NOTE: EXEC_CYCLE_STOP is set by the stepper subsystem when a cycle or feed hold completes. + if (rt_exec & EXEC_CYCLE_STOP) { + if (sys.state != STATE_QUEUED) { + sys.state = STATE_IDLE; + } + bit_false(sys.execute,EXEC_CYCLE_STOP); + } + + } + + // Overrides flag byte (sys.override) and execution should be installed here, since they + // are runtime and require a direct and controlled interface to the main stepper program. + + // Reload step segment buffer + if (sys.state & (STATE_CYCLE | STATE_HOLD | STATE_HOMING)) { st_prep_buffer(); } + +} + + +// Block until all buffered steps are executed or in a cycle state. Works with feed hold +// during a synchronize call, if it should happen. Also, waits for clean cycle end. +void protocol_buffer_synchronize() +{ + // Check and set auto start to resume cycle after synchronize and caller completes. + if (sys.state == STATE_CYCLE) { sys.auto_start = true; } + while (plan_get_current_block() || (sys.state == STATE_CYCLE)) { + protocol_execute_runtime(); // Check and execute run-time commands + if (sys.abort) { return; } // Check for system abort + } +} + + +// Auto-cycle start has two purposes: 1. Resumes a plan_synchronize() call from a function that +// requires the planner buffer to empty (spindle enable, dwell, etc.) 2. As a user setting that +// automatically begins the cycle when a user enters a valid motion command manually. This is +// intended as a beginners feature to help new users to understand g-code. It can be disabled +// as a beginner tool, but (1.) still operates. If disabled, the operation of cycle start is +// manually issuing a cycle start command whenever the user is ready and there is a valid motion +// command in the planner queue. +// NOTE: This function is called from the main loop and mc_line() only and executes when one of +// two conditions exist respectively: There are no more blocks sent (i.e. streaming is finished, +// single commands), or the planner buffer is full and ready to go. +void protocol_auto_cycle_start() { if (sys.auto_start) { sys.execute |= EXEC_CYCLE_START; } } diff --git a/protocol.h b/protocol.h index 9107d1d..7bafba2 100644 --- a/protocol.h +++ b/protocol.h @@ -1,5 +1,5 @@ /* - protocol.h - controls Grbl execution procedures + protocol.h - controls Grbl execution protocol and procedures Part of Grbl Copyright (c) 2011-2014 Sungeun K. Jeon @@ -31,11 +31,26 @@ #define LINE_BUFFER_SIZE 70 #endif +// Starts Grbl main loop. It handles all incoming characters from the serial port and executes +// them as they complete. It is also responsible for finishing the initialization procedures. +void protocol_main_loop(); + // Checks and executes a runtime command at various stop points in main program void protocol_execute_runtime(); -// Starts Grbl main loop. It handles all incoming characters from the serial port and executes -// them as they complete. It is also responsible for finishing the initialization procedures. -void protocol_process(); +// Notify the stepper subsystem to start executing the g-code program in buffer. +// void protocol_cycle_start(); + +// Reinitializes the buffer after a feed hold for a resume. +// void protocol_cycle_reinitialize(); + +// Initiates a feed hold of the running program +// void protocol_feed_hold(); + +// Executes the auto cycle feature, if enabled. +void protocol_auto_cycle_start(); + +// Block until all buffered steps are executed +void protocol_buffer_synchronize(); #endif diff --git a/report.c b/report.c index ee4de61..5416651 100644 --- a/report.c +++ b/report.c @@ -32,6 +32,7 @@ #include "settings.h" #include "gcode.h" #include "coolant_control.h" +#include "spindle_control.h" // Handles the primary confirmation protocol response for streaming interfaces and human-feedback. @@ -260,9 +261,9 @@ void report_gcode_modes() } switch (gc.spindle_direction) { - case 1 : printPgmString(PSTR(" M3")); break; - case -1 : printPgmString(PSTR(" M4")); break; - case 0 : printPgmString(PSTR(" M5")); break; + case SPINDLE_ENABLE_CW : printPgmString(PSTR(" M3")); break; + case SPINDLE_ENABLE_CCW : printPgmString(PSTR(" M4")); break; + case SPINDLE_DISABLE : printPgmString(PSTR(" M5")); break; } switch (gc.coolant_mode) { diff --git a/spindle_control.c b/spindle_control.c index e97c3f7..ad3efc9 100644 --- a/spindle_control.c +++ b/spindle_control.c @@ -21,23 +21,21 @@ #include "system.h" #include "spindle_control.h" -#include "planner.h" +#include "protocol.h" void spindle_init() -{ - SPINDLE_DIRECTION_DDR |= (1<prescaler<cycles_per_tick; st.step_count = st.exec_segment->n_step; // NOTE: Can sometimes be zero when moving slow. // If the new segment starts a new planner block, initialize stepper variables and counters. @@ -310,17 +322,22 @@ ISR(TIMER1_COMPA_vect) if ( st.exec_block_index != st.exec_segment->st_block_index ) { st.exec_block_index = st.exec_segment->st_block_index; st.exec_block = &st_block_buffer[st.exec_block_index]; - st.dir_outbits = st.exec_block->direction_bits ^ settings.dir_invert_mask; + // Initialize Bresenham line and distance counters st.counter_x = (st.exec_block->step_event_count >> 1); st.counter_y = st.counter_x; st.counter_z = st.counter_x; } + + st.dir_outbits = st.exec_block->direction_bits ^ settings.dir_invert_mask; + #ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING + // With AMASS enabled, adjust Bresenham axis increment counters according to AMASS level. st.steps[X_AXIS] = st.exec_block->steps[X_AXIS] >> st.exec_segment->amass_level; st.steps[Y_AXIS] = st.exec_block->steps[Y_AXIS] >> st.exec_segment->amass_level; st.steps[Z_AXIS] = st.exec_block->steps[Z_AXIS] >> st.exec_segment->amass_level; #endif + } else { // Segment buffer empty. Shutdown. st_go_idle(); @@ -379,28 +396,27 @@ ISR(TIMER1_COMPA_vect) st.step_outbits ^= settings.step_invert_mask; // Apply step port invert mask busy = false; -// SPINDLE_ENABLE_PORT ^= 1<direction_bits = pl_block->direction_bits; #ifndef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING @@ -565,6 +529,9 @@ void st_prep_buffer() st_prep_block->steps[Z_AXIS] = pl_block->steps[Z_AXIS]; st_prep_block->step_event_count = pl_block->step_event_count; #else + // With AMASS enabled, simply bit-shift multiply all Bresenham data by the max AMASS + // level, such that we never divide beyond the original data anywhere in the algorithm. + // If the original data is divided, we can lose a step from integer roundoff. st_prep_block->steps[X_AXIS] = pl_block->steps[X_AXIS] << MAX_AMASS_LEVEL; st_prep_block->steps[Y_AXIS] = pl_block->steps[Y_AXIS] << MAX_AMASS_LEVEL; st_prep_block->steps[Z_AXIS] = pl_block->steps[Z_AXIS] << MAX_AMASS_LEVEL; @@ -574,8 +541,9 @@ void st_prep_buffer() // Initialize segment buffer data for generating the segments. prep.steps_remaining = pl_block->step_event_count; prep.step_per_mm = prep.steps_remaining/pl_block->millimeters; - prep.mm_per_step = pl_block->millimeters/prep.steps_remaining; - prep.minimum_mm = pl_block->millimeters-prep.mm_per_step; + prep.req_mm_increment = REQ_MM_INCREMENT_SCALAR*pl_block->millimeters/prep.steps_remaining; + + prep.dt_remainder = 0.0; // Reset for new planner block if (sys.state == STATE_HOLD) { // Override planner block entry speed and enforce deceleration during feed hold. @@ -593,18 +561,20 @@ void st_prep_buffer() */ prep.mm_complete = 0.0; // Default velocity profile complete at 0.0mm from end of block. float inv_2_accel = 0.5/pl_block->acceleration; - if (sys.state == STATE_HOLD) { + if (sys.state == STATE_HOLD) { // [Forced Deceleration to Zero Velocity] // Compute velocity profile parameters for a feed hold in-progress. This profile overrides // the planner block profile, enforcing a deceleration to zero speed. prep.ramp_type = RAMP_DECEL; - float decel_dist = inv_2_accel*pl_block->entry_speed_sqr; - if (decel_dist < pl_block->millimeters) { - prep.exit_speed = 0.0; - prep.mm_complete = pl_block->millimeters-decel_dist; // End of feed hold. - } else { + // Compute decelerate distance relative to end of block. + float decel_dist = pl_block->millimeters - inv_2_accel*pl_block->entry_speed_sqr; + if (decel_dist < 0.0) { + // Deceleration through entire planner block. End of feed hold is not in this block. prep.exit_speed = sqrt(pl_block->entry_speed_sqr-2*pl_block->acceleration*pl_block->millimeters); + } else { + prep.mm_complete = decel_dist; // End of feed hold. + prep.exit_speed = 0.0; } - } else { + } else { // [Normal Operation] // Compute or recompute velocity profile parameters of the prepped planner block. prep.ramp_type = RAMP_ACCEL; // Initialize as acceleration ramp. prep.accelerate_until = pl_block->millimeters; @@ -640,7 +610,8 @@ void st_prep_buffer() // prep.decelerate_after = 0.0; prep.maximum_speed = prep.exit_speed; } - } + } + } // Initialize new segment @@ -649,6 +620,24 @@ void st_prep_buffer() // Set new segment to point to the current segment data block. prep_segment->st_block_index = prep.st_block_index; + + float mm_remaining = pl_block->millimeters; + float minimum_mm = pl_block->millimeters-prep.req_mm_increment; + if (minimum_mm < 0.0) { minimum_mm = 0.0; } + if (sys.state == STATE_HOLD) { + if (minimum_mm < prep.mm_complete) { // NOTE: Exit condition + // Less than one step to decelerate to zero speed, but already very close. AMASS + // requires full steps to execute. So, just bail. + prep.current_speed = 0.0; + prep.dt_remainder = 0.0; + prep.steps_remaining = ceil(pl_block->millimeters * prep.step_per_mm); + pl_block->millimeters = prep.steps_remaining/prep.step_per_mm; // Update with full steps. + plan_cycle_reinitialize(); + sys.state = STATE_QUEUED; + return; // Segment not generated, but current step data still retained. + } + } + /*------------------------------------------------------------------------------------ Compute the average velocity of this new segment by determining the total distance traveled over the segment time DT_SEGMENT. The following code first attempts to create @@ -663,12 +652,11 @@ void st_prep_buffer() the end of planner block (typical) or mid-block at the end of a forced deceleration, such as from a feed hold. */ - float dt_max = DT_SEGMENT; // Set maximum segment time + float dt_max = DT_SEGMENT; // Maximum segment time float dt = 0.0; // Initialize segment time - float mm_remaining = pl_block->millimeters; float time_var = dt_max; // Time worker variable float mm_var; // mm-Distance worker variable - float speed_var; // Speed worker variable. + float speed_var; // Speed worker variable do { switch (prep.ramp_type) { case RAMP_ACCEL: @@ -716,7 +704,7 @@ void st_prep_buffer() dt += time_var; // Add computed ramp time to total segment time. if (dt < dt_max) { time_var = dt_max - dt; } // **Incomplete** At ramp junction. else { - if (mm_remaining > prep.minimum_mm) { // Check for slow segments with zero steps. + if (mm_remaining > minimum_mm) { // Check for very slow segments with zero steps. dt_max += DT_SEGMENT; // Increase segment time to ensure at least one step in segment. time_var = dt_max - dt; } else { @@ -724,9 +712,10 @@ void st_prep_buffer() } } } while (mm_remaining > prep.mm_complete); // **Complete** Exit loop. Profile complete. + /* ----------------------------------------------------------------------------------- - Compute segment step rate, steps to execute, and step phase correction parameters. + Compute segment step rate, steps to execute, and apply necessary rate corrections. NOTE: Steps are computed by direct scalar conversion of the millimeter distance remaining in the block, rather than incrementally tallying the steps executed per segment. This helps in removing floating point round-off issues of several additions. @@ -735,15 +724,26 @@ void st_prep_buffer() Fortunately, this scenario is highly unlikely and unrealistic in CNC machines supported by Grbl (i.e. exceeding 10 meters axis travel at 200 step/mm). */ - uint32_t cycles; - float steps_remaining = prep.step_per_mm*mm_remaining; + float steps_remaining = prep.step_per_mm*mm_remaining; // Convert mm_remaining to steps + float n_steps_remaining = ceil(steps_remaining); // Round-up current steps remaining + float last_n_steps_remaining = ceil(prep.steps_remaining); // Round-up last steps remaining + prep_segment->n_step = last_n_steps_remaining-n_steps_remaining; // Compute number of steps to execute. - // Compute number of steps to execute and segment step phase correction. - prep_segment->n_step = ceil(prep.steps_remaining)-ceil(steps_remaining); + // Compute segment step rate. Since steps are integers and mm distances traveled are not, + // the end of every segment can have a partial step of varying magnitudes that are not + // executed, because the stepper ISR requires whole steps due to the AMASS algorithm. To + // compensate, we track the time to execute the previous segment's partial step and simply + // apply it with the partial step distance to the current segment, so that it minutely + // adjusts the whole segment rate to keep step output exact. These rate adjustments are + // typically very small and do not adversely effect performance, but ensures that Grbl + // outputs the exact acceleration and velocity profiles as computed by the planner. + dt += prep.dt_remainder; // Apply previous segment partial step execute time + float inv_rate = dt/(last_n_steps_remaining - steps_remaining); // Compute adjusted step rate inverse + prep.dt_remainder = (n_steps_remaining - steps_remaining)*inv_rate; // Update segment partial step time + + // Compute CPU cycles per step for the prepped segment. + uint32_t cycles = ceil( (TICKS_PER_MICROSECOND*1000000*60)*inv_rate ); // (cycles/step) - float inv_rate = dt/(prep.steps_remaining-steps_remaining); - cycles = ceil( (TICKS_PER_MICROSECOND*1000000*60)*inv_rate ); // (cycles/step) - #ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING // Compute step timing and multi-axis smoothing level. // NOTE: AMASS overdrives the timer with each level, so only one prescalar is required. @@ -777,32 +777,27 @@ void st_prep_buffer() // Determine end of segment conditions. Setup initial conditions for next segment. if (mm_remaining > prep.mm_complete) { - // Normal operation. Block incomplete. Distance remaining to be executed. - prep.minimum_mm = prep.mm_per_step*floor(steps_remaining); + // Normal operation. Block incomplete. Distance remaining in block to be executed. pl_block->millimeters = mm_remaining; prep.steps_remaining = steps_remaining; } else { // End of planner block or forced-termination. No more distance to be executed. if (mm_remaining > 0.0) { // At end of forced-termination. - // NOTE: Currently only feed holds qualify for this scenario. May change with overrides. + // Reset prep parameters for resuming and then bail. + // NOTE: Currently only feed holds qualify for this scenario. May change with overrides. prep.current_speed = 0.0; + prep.dt_remainder = 0.0; prep.steps_remaining = ceil(steps_remaining); - prep.minimum_mm = prep.steps_remaining-prep.mm_per_step; - pl_block->millimeters = prep.steps_remaining*prep.mm_per_step; // Update with full steps. - plan_cycle_reinitialize(); - sys.state = STATE_QUEUED; // End cycle. + pl_block->millimeters = prep.steps_remaining/prep.step_per_mm; // Update with full steps. + plan_cycle_reinitialize(); + sys.state = STATE_QUEUED; // End cycle. + +// TODO: Try to move QUEUED setting into cycle re-initialize. + } else { // End of planner block // The planner block is complete. All steps are set to be executed in the segment buffer. pl_block = NULL; plan_discard_current_block(); - - if (sys.state == STATE_HOLD) { - if (prep.current_speed == 0.0) { -// TODO: Check if the segment buffer gets initialized correctly. - plan_cycle_reinitialize(); - sys.state = STATE_QUEUED; - } - } } } @@ -810,6 +805,8 @@ void st_prep_buffer() segment_buffer_head = segment_next_head; if ( ++segment_next_head == SEGMENT_BUFFER_SIZE ) { segment_next_head = 0; } + if (sys.state == STATE_QUEUED) { return; } // Bail if hold completes + // int32_t blength = segment_buffer_head - segment_buffer_tail; // if (blength < 0) { blength += SEGMENT_BUFFER_SIZE; } // printInteger(blength); diff --git a/stepper.h b/stepper.h index fe9323f..1cc06d9 100644 --- a/stepper.h +++ b/stepper.h @@ -38,15 +38,6 @@ void st_go_idle(); // Reset the stepper subsystem variables void st_reset(); -// Notify the stepper subsystem to start executing the g-code program in buffer. -void st_cycle_start(); - -// Reinitializes the buffer after a feed hold for a resume. -void st_cycle_reinitialize(); - -// Initiates a feed hold of the running program -void st_feed_hold(); - // Reloads step segment buffer. Called continuously by runtime execution system. void st_prep_buffer(); diff --git a/system.c b/system.c index 4dfe31d..229203f 100644 --- a/system.c +++ b/system.c @@ -28,7 +28,7 @@ void system_init() { - PINOUT_DDR &= ~(PINOUT_MASK); // Set as input pins + PINOUT_DDR &= ~(PINOUT_MASK); // Configure as input pins PINOUT_PORT |= PINOUT_MASK; // Enable internal pull-up resistors. Normal high operation. PINOUT_PCMSK |= PINOUT_MASK; // Enable specific pins of the Pin Change Interrupt PCICR |= (1 << PINOUT_INT); // Enable Pin Change Interrupt @@ -85,6 +85,7 @@ uint8_t system_execute_line(char *line) uint8_t helper_var = 0; // Helper variable float parameter, value; switch( line[char_counter] ) { + case 0 : report_grbl_help(); break; case '#' : // Print gcode parameters if ( line[++char_counter] != 0 ) { return(STATUS_UNSUPPORTED_STATEMENT); } else { report_gcode_parameters(); } @@ -93,7 +94,29 @@ uint8_t system_execute_line(char *line) if ( line[++char_counter] != 0 ) { return(STATUS_UNSUPPORTED_STATEMENT); } else { report_gcode_modes(); } break; -// case 'J' : break; // Jogging methods + case 'C' : // Set check g-code mode [IDLE/CHECK] + if ( line[++char_counter] != 0 ) { return(STATUS_UNSUPPORTED_STATEMENT); } + // Perform reset when toggling off. Check g-code mode should only work if Grbl + // is idle and ready, regardless of alarm locks. This is mainly to keep things + // simple and consistent. + if ( sys.state == STATE_CHECK_MODE ) { + mc_reset(); + report_feedback_message(MESSAGE_DISABLED); + } else { + if (sys.state) { return(STATUS_IDLE_ERROR); } // Requires no alarm mode. + sys.state = STATE_CHECK_MODE; + report_feedback_message(MESSAGE_ENABLED); + } + break; + case 'X' : // Disable alarm lock [ALARM] + if ( line[++char_counter] != 0 ) { return(STATUS_UNSUPPORTED_STATEMENT); } + if (sys.state == STATE_ALARM) { + report_feedback_message(MESSAGE_ALARM_UNLOCK); + sys.state = STATE_IDLE; + // Don't run startup script. Prevents stored moves in startup from causing accidents. + } // Otherwise, no effect. + break; +// case 'J' : break; // Jogging methods // TODO: Here jogging can be placed for execution as a seperate subprogram. It does not need to be // susceptible to other runtime commands except for e-stop. The jogging function is intended to // be a basic toggle on/off with controlled acceleration and deceleration to prevent skipped @@ -109,41 +132,18 @@ uint8_t system_execute_line(char *line) // Block any system command that requires the state as IDLE/ALARM. (i.e. EEPROM, homing) if ( !(sys.state == STATE_IDLE || sys.state == STATE_ALARM) ) { return(STATUS_IDLE_ERROR); } switch( line[char_counter] ) { - case 0 : report_grbl_help(); break; - case '$' : // Prints Grbl settings + case '$' : // Prints Grbl settings [IDLE/ALARM] if ( line[++char_counter] != 0 ) { return(STATUS_UNSUPPORTED_STATEMENT); } else { report_grbl_settings(); } break; - case 'C' : // Set check g-code mode - if ( line[++char_counter] != 0 ) { return(STATUS_UNSUPPORTED_STATEMENT); } - // Perform reset when toggling off. Check g-code mode should only work if Grbl - // is idle and ready, regardless of alarm locks. This is mainly to keep things - // simple and consistent. - if ( sys.state == STATE_CHECK_MODE ) { - mc_reset(); - report_feedback_message(MESSAGE_DISABLED); - } else { - if (sys.state) { return(STATUS_IDLE_ERROR); } - sys.state = STATE_CHECK_MODE; - report_feedback_message(MESSAGE_ENABLED); - } - break; - case 'X' : // Disable alarm lock - if ( line[++char_counter] != 0 ) { return(STATUS_UNSUPPORTED_STATEMENT); } - if (sys.state == STATE_ALARM) { - report_feedback_message(MESSAGE_ALARM_UNLOCK); - sys.state = STATE_IDLE; - // Don't run startup script. Prevents stored moves in startup from causing accidents. - } - break; - case 'H' : // Perform homing cycle + case 'H' : // Perform homing cycle [IDLE/ALARM] if (bit_istrue(settings.flags,BITFLAG_HOMING_ENABLE)) { // Only perform homing if Grbl is idle or lost. mc_homing_cycle(); if (!sys.abort) { system_execute_startup(line); } // Execute startup scripts after successful homing. } else { return(STATUS_SETTING_DISABLED); } break; - case 'I' : // Print or store build info. + case 'I' : // Print or store build info. [IDLE/ALARM] if ( line[++char_counter] == 0 ) { if (!(settings_read_build_info(line))) { report_status_message(STATUS_SETTING_READ_FAIL); @@ -159,7 +159,7 @@ uint8_t system_execute_line(char *line) settings_store_build_info(line); } break; - case 'N' : // Startup lines. + case 'N' : // Startup lines. [IDLE/ALARM] if ( line[++char_counter] == 0 ) { // Print startup lines for (helper_var=0; helper_var < N_STARTUP_LINE; helper_var++) { if (!(settings_read_startup_line(helper_var, line))) { @@ -170,10 +170,11 @@ uint8_t system_execute_line(char *line) } break; } else { // Store startup line + if (sys.state != STATE_IDLE) { return(STATUS_IDLE_ERROR); } // Store only when idle. helper_var = true; // Set helper_var to flag storing method. // No break. Continues into default: to read remaining command characters. } - default : // Storing setting methods + default : // Storing setting methods [IDLE/ALARM] if(!read_float(line, &char_counter, ¶meter)) { return(STATUS_BAD_NUMBER_FORMAT); } if(line[char_counter++] != '=') { return(STATUS_UNSUPPORTED_STATEMENT); } if (helper_var) { // Store startup line From 3df61e0ec5f7cb0b4c09a847558ecf14a8697802 Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Sat, 15 Feb 2014 13:13:46 -0700 Subject: [PATCH 43/73] Homing and feed hold bug fixes. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit WARNING: Bugs may still exist. This branch is a work in progress and will be pushed to the edge branch when at beta stability. Use at your own risk. - Homing freezing issue fixed. Had to do with the cycle stop flag being set incorrectly after the homing cycles and before the pull-off maneuver. Now resets the stepper motors before this can happen. - Fixed an issue with a rare feed hold failure. Had to do with feed hold ending exactly at the end of a block. The runtime protocol now sets the QUEUED and IDLE states appropriately when this occurs. Still need to clean this code up however, as it’s patched rather than written well. - Updated version build via $I command. - Forgot to comment on a new feature for the last commit. Since steps are integers and millimeters traveled are floats, the old step segment generator ignored the step fraction differences in generating the segment velocities. Didn’t see like it would be much of a big deal, but there were instances that this would be a problem, especially for very slow feed rates. The stepper algorithm now micro-adjusts the segment velocities based on the step fractions not executed from the previous segment. This ensures that Grbl generates the velocity profiles EXACTLY and noticeably improves overall acceleration performance. --- limits.c | 13 ++++++------- protocol.c | 5 ++--- settings.h | 2 +- spindle_control.c | 8 ++++---- stepper.c | 39 ++++++++++++++++++++------------------- 5 files changed, 33 insertions(+), 34 deletions(-) diff --git a/limits.c b/limits.c index 82a9e57..9016e8a 100644 --- a/limits.c +++ b/limits.c @@ -189,16 +189,16 @@ void limits_go_home(uint8_t cycle_mask) // Check only for user reset. No time to run protocol_execute_runtime() in this loop. if (sys.execute & EXEC_RESET) { protocol_execute_runtime(); return; } } while (STEP_MASK & axislock); - - delay_ms(settings.homing_debounce_delay); // Delay to allow transient dynamics to dissipate. - - // Reverse direction and reset homing rate for locate cycle(s). - homing_rate = settings.homing_feed_rate; - approach = !approach; st_reset(); // Force disable steppers and reset step segment buffer. Ensure homing motion is cleared. plan_reset(); // Reset planner buffer. Zero planner positions. Ensure homing motion is cleared. + delay_ms(settings.homing_debounce_delay); // Delay to allow transient dynamics to dissipate. + + // Reverse direction and reset homing rate for locate cycle(s). + homing_rate = settings.homing_feed_rate; + approach = !approach; + } while (n_cycle-- > 0); // The active cycle axes should now be homed and machine limits have been located. By @@ -230,7 +230,6 @@ void limits_go_home(uint8_t cycle_mask) // Initiate pull-off using main motion control routines. // TODO : Clean up state routines so that this motion still shows homing state. sys.state = STATE_QUEUED; -// protocol_cycle_start(); sys.execute |= EXEC_CYCLE_START; protocol_execute_runtime(); protocol_buffer_synchronize(); // Complete pull-off motion. diff --git a/protocol.c b/protocol.c index 7c7ae2c..c8406d0 100644 --- a/protocol.c +++ b/protocol.c @@ -242,9 +242,8 @@ void protocol_execute_runtime() // cycle reinitializations. The stepper path should continue exactly as if nothing has happened. // NOTE: EXEC_CYCLE_STOP is set by the stepper subsystem when a cycle or feed hold completes. if (rt_exec & EXEC_CYCLE_STOP) { - if (sys.state != STATE_QUEUED) { - sys.state = STATE_IDLE; - } + if ( plan_get_current_block() ) { sys.state = STATE_QUEUED; } + else { sys.state = STATE_IDLE; } bit_false(sys.execute,EXEC_CYCLE_STOP); } diff --git a/settings.h b/settings.h index 6ff54be..3551ec2 100644 --- a/settings.h +++ b/settings.h @@ -26,7 +26,7 @@ #define GRBL_VERSION "0.9c" -#define GRBL_VERSION_BUILD "20140110" +#define GRBL_VERSION_BUILD "20140215" // Version of the EEPROM data. Will be used to migrate existing data from older versions of Grbl // when firmware is upgraded. Always stored in byte 0 of eeprom diff --git a/spindle_control.c b/spindle_control.c index ad3efc9..16034b0 100644 --- a/spindle_control.c +++ b/spindle_control.c @@ -61,11 +61,11 @@ void spindle_run(uint8_t direction, float rpm) // Halt or set spindle direction and rpm. if (direction == SPINDLE_DISABLE) { - + spindle_stop(); - + } else { - + if (direction == SPINDLE_ENABLE_CW) { SPINDLE_DIRECTION_PORT &= ~(1< SPINDLE_RPM_RANGE ) { rpm = SPINDLE_RPM_RANGE; } // Prevent uint8 overflow uint8_t current_pwm = floor( rpm*(255.0/SPINDLE_RPM_RANGE) + 0.5); OCR_REGISTER = current_pwm; - + #ifndef CPU_MAP_ATMEGA328P // On the Uno, spindle enable and PWM are shared. SPINDLE_ENABLE_PORT |= (1<st_block_index = prep.st_block_index; - - float mm_remaining = pl_block->millimeters; - float minimum_mm = pl_block->millimeters-prep.req_mm_increment; - if (minimum_mm < 0.0) { minimum_mm = 0.0; } - if (sys.state == STATE_HOLD) { - if (minimum_mm < prep.mm_complete) { // NOTE: Exit condition - // Less than one step to decelerate to zero speed, but already very close. AMASS - // requires full steps to execute. So, just bail. - prep.current_speed = 0.0; - prep.dt_remainder = 0.0; - prep.steps_remaining = ceil(pl_block->millimeters * prep.step_per_mm); - pl_block->millimeters = prep.steps_remaining/prep.step_per_mm; // Update with full steps. - plan_cycle_reinitialize(); - sys.state = STATE_QUEUED; - return; // Segment not generated, but current step data still retained. - } - } - /*------------------------------------------------------------------------------------ Compute the average velocity of this new segment by determining the total distance traveled over the segment time DT_SEGMENT. The following code first attempts to create @@ -656,7 +638,11 @@ void st_prep_buffer() float dt = 0.0; // Initialize segment time float time_var = dt_max; // Time worker variable float mm_var; // mm-Distance worker variable - float speed_var; // Speed worker variable + float speed_var; // Speed worker variable + float mm_remaining = pl_block->millimeters; + float minimum_mm = pl_block->millimeters-prep.req_mm_increment; + if (minimum_mm < 0.0) { minimum_mm = 0.0; } + do { switch (prep.ramp_type) { case RAMP_ACCEL: @@ -728,6 +714,21 @@ void st_prep_buffer() float n_steps_remaining = ceil(steps_remaining); // Round-up current steps remaining float last_n_steps_remaining = ceil(prep.steps_remaining); // Round-up last steps remaining prep_segment->n_step = last_n_steps_remaining-n_steps_remaining; // Compute number of steps to execute. + + // Bail if we are at the end of a feed hold and don't have a step to execute. + if (sys.state == STATE_HOLD) { + if (prep_segment->n_step == 0) { + // Less than one step to decelerate to zero speed, but already very close. AMASS + // requires full steps to execute. So, just bail. + prep.current_speed = 0.0; + prep.dt_remainder = 0.0; + prep.steps_remaining = n_steps_remaining; + pl_block->millimeters = prep.steps_remaining/prep.step_per_mm; // Update with full steps. + plan_cycle_reinitialize(); + sys.state = STATE_QUEUED; + return; // Segment not generated, but current step data still retained. + } + } // Compute segment step rate. Since steps are integers and mm distances traveled are not, // the end of every segment can have a partial step of varying magnitudes that are not From b332d6edbba7b147fba7be4779a68a44ad9c5df1 Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Wed, 19 Feb 2014 07:21:40 -0700 Subject: [PATCH 44/73] Commenting updates. Minor bug fix with exit of soft limit event. --- limits.c | 4 ++-- motion_control.c | 21 ++++++++++++--------- stepper.c | 28 +++++++++++++++------------- system.c | 4 ++-- system.h | 12 ++++++------ 5 files changed, 37 insertions(+), 32 deletions(-) diff --git a/limits.c b/limits.c index 9016e8a..1660ec4 100644 --- a/limits.c +++ b/limits.c @@ -190,7 +190,7 @@ void limits_go_home(uint8_t cycle_mask) if (sys.execute & EXEC_RESET) { protocol_execute_runtime(); return; } } while (STEP_MASK & axislock); - st_reset(); // Force disable steppers and reset step segment buffer. Ensure homing motion is cleared. + st_reset(); // Immediately force kill steppers and reset step segment buffer. plan_reset(); // Reset planner buffer. Zero planner positions. Ensure homing motion is cleared. delay_ms(settings.homing_debounce_delay); // Delay to allow transient dynamics to dissipate. @@ -255,7 +255,7 @@ void limits_soft_check(float *target) do { protocol_execute_runtime(); if (sys.abort) { return; } - } while (sys.state == STATE_HOLD); + } while ( sys.state != STATE_IDLE || sys.state != STATE_QUEUED); } mc_reset(); // Issue system reset and ensure spindle and coolant are shutdown. diff --git a/motion_control.c b/motion_control.c index b3711a9..f9b1c7b 100644 --- a/motion_control.c +++ b/motion_control.c @@ -48,16 +48,19 @@ void mc_line(float *target, float feed_rate, uint8_t invert_feed_rate) // If in check gcode mode, prevent motion by blocking planner. Soft limits still work. if (sys.state == STATE_CHECK_MODE) { return; } - // TODO: Backlash compensation may be installed here. Only need direction info to track when - // to insert a backlash line motion(s) before the intended line motion. Requires its own + // NOTE: Backlash compensation may be installed here. It will need direction info to track when + // to insert a backlash line motion(s) before the intended line motion and will require its own // plan_check_full_buffer() and check for system abort loop. Also for position reporting - // backlash steps will need to be also tracked. Not sure what the best strategy is for this, - // i.e. keep the planner independent and do the computations in the status reporting, or let - // the planner handle the position corrections. The latter may get complicated. - // TODO: Backlash comp positioning values may need to be kept at a system level, i.e. tracking - // true position after a feed hold in the middle of a backlash move. The difficulty is in making - // sure that the stepper subsystem and planner are working in sync, and the status report - // position also takes this into account. + // backlash steps will need to be also tracked, which will need to be kept at a system level. + // There are likely some other things that will need to be tracked as well. However, we feel + // that backlash compensation should NOT be handled by Grbl itself, because there are a myriad + // of ways to implement it and can be effective or ineffective for different CNC machines. This + // would be better handled by the interface as a post-processor task, where the original g-code + // is translated and inserts backlash motions that best suits the machine. + // NOTE: Perhaps as a middle-ground, all that needs to be sent is a flag or special command that + // indicates to Grbl what is a backlash compensation motion, so that Grbl executes the move but + // doesn't update the machine position values. Since the position values used by the g-code + // parser and planner are separate from the system machine positions, this is doable. // If the buffer is full: good! That means we are well ahead of the robot. // Remain in this loop until there is room in the buffer. diff --git a/stepper.c b/stepper.c index 54a6ecb..bfe1f74 100644 --- a/stepper.c +++ b/stepper.c @@ -541,7 +541,7 @@ void st_prep_buffer() // Initialize segment buffer data for generating the segments. prep.steps_remaining = pl_block->step_event_count; prep.step_per_mm = prep.steps_remaining/pl_block->millimeters; - prep.req_mm_increment = REQ_MM_INCREMENT_SCALAR*pl_block->millimeters/prep.steps_remaining; + prep.req_mm_increment = REQ_MM_INCREMENT_SCALAR/prep.step_per_mm; prep.dt_remainder = 0.0; // Reset for new planner block @@ -639,8 +639,8 @@ void st_prep_buffer() float time_var = dt_max; // Time worker variable float mm_var; // mm-Distance worker variable float speed_var; // Speed worker variable - float mm_remaining = pl_block->millimeters; - float minimum_mm = pl_block->millimeters-prep.req_mm_increment; + float mm_remaining = pl_block->millimeters; // New segment distance from end of block. + float minimum_mm = mm_remaining-prep.req_mm_increment; // Guarantee at least one step. if (minimum_mm < 0.0) { minimum_mm = 0.0; } do { @@ -691,7 +691,9 @@ void st_prep_buffer() if (dt < dt_max) { time_var = dt_max - dt; } // **Incomplete** At ramp junction. else { if (mm_remaining > minimum_mm) { // Check for very slow segments with zero steps. - dt_max += DT_SEGMENT; // Increase segment time to ensure at least one step in segment. + // Increase segment time to ensure at least one step in segment. Override and loop + // through distance calculations until minimum_mm or mm_complete. + dt_max += DT_SEGMENT; time_var = dt_max - dt; } else { break; // **Complete** Exit loop. Segment execution time maxed. @@ -716,8 +718,9 @@ void st_prep_buffer() prep_segment->n_step = last_n_steps_remaining-n_steps_remaining; // Compute number of steps to execute. // Bail if we are at the end of a feed hold and don't have a step to execute. - if (sys.state == STATE_HOLD) { - if (prep_segment->n_step == 0) { + if (prep_segment->n_step == 0) { + if (sys.state == STATE_HOLD) { + // Less than one step to decelerate to zero speed, but already very close. AMASS // requires full steps to execute. So, just bail. prep.current_speed = 0.0; @@ -776,7 +779,11 @@ void st_prep_buffer() } #endif - // Determine end of segment conditions. Setup initial conditions for next segment. + // Segment complete! Increment segment buffer indices. + segment_buffer_head = segment_next_head; + if ( ++segment_next_head == SEGMENT_BUFFER_SIZE ) { segment_next_head = 0; } + + // Setup initial conditions for next segment. if (mm_remaining > prep.mm_complete) { // Normal operation. Block incomplete. Distance remaining in block to be executed. pl_block->millimeters = mm_remaining; @@ -793,6 +800,7 @@ void st_prep_buffer() plan_cycle_reinitialize(); sys.state = STATE_QUEUED; // End cycle. + return; // Bail! // TODO: Try to move QUEUED setting into cycle re-initialize. } else { // End of planner block @@ -802,12 +810,6 @@ void st_prep_buffer() } } - // New step segment initialization completed. Increment segment buffer indices. - segment_buffer_head = segment_next_head; - if ( ++segment_next_head == SEGMENT_BUFFER_SIZE ) { segment_next_head = 0; } - - if (sys.state == STATE_QUEUED) { return; } // Bail if hold completes - // int32_t blength = segment_buffer_head - segment_buffer_tail; // if (blength < 0) { blength += SEGMENT_BUFFER_SIZE; } // printInteger(blength); diff --git a/system.c b/system.c index 229203f..110fa82 100644 --- a/system.c +++ b/system.c @@ -150,7 +150,7 @@ uint8_t system_execute_line(char *line) } else { report_build_info(line); } - } else { // Store startup line + } else { // Store startup line [IDLE/ALARM] if(line[char_counter++] != '=') { return(STATUS_UNSUPPORTED_STATEMENT); } helper_var = char_counter; // Set helper variable as counter to start of user info line. do { @@ -169,7 +169,7 @@ uint8_t system_execute_line(char *line) } } break; - } else { // Store startup line + } else { // Store startup line [IDLE Only] Prevents motion during ALARM. if (sys.state != STATE_IDLE) { return(STATUS_IDLE_ERROR); } // Store only when idle. helper_var = true; // Set helper_var to flag storing method. // No break. Continues into default: to read remaining command characters. diff --git a/system.h b/system.h index 78faa5c..12d8965 100644 --- a/system.h +++ b/system.h @@ -59,12 +59,12 @@ // of Grbl to manage each without overlapping. It is also used as a messaging flag for // critical events. #define STATE_IDLE 0 // Must be zero. No flags. -#define STATE_QUEUED bit(0) // Indicates buffered blocks, awaiting cycle start. -#define STATE_CYCLE bit(1) // Cycle is running -#define STATE_HOLD bit(2) // Executing feed hold -#define STATE_HOMING bit(3) // Performing homing cycle -#define STATE_ALARM bit(4) // In alarm state. Locks out all g-code processes. Allows settings access. -#define STATE_CHECK_MODE bit(5) // G-code check mode. Locks out planner and motion only. +#define STATE_ALARM bit(0) // In alarm state. Locks out all g-code processes. Allows settings access. +#define STATE_CHECK_MODE bit(1) // G-code check mode. Locks out planner and motion only. +#define STATE_HOMING bit(2) // Performing homing cycle +#define STATE_QUEUED bit(3) // Indicates buffered blocks, awaiting cycle start. +#define STATE_CYCLE bit(4) // Cycle is running +#define STATE_HOLD bit(5) // Executing feed hold // #define STATE_JOG bit(6) // Jogging mode is unique like homing. // Define global system variables From 1fd45791a52cda924683d88311c108908c51b598 Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Wed, 19 Feb 2014 07:48:09 -0700 Subject: [PATCH 45/73] Minor updates to line number feature. - Changed line number integer types from unsigned to signed int32. G-code mandates values cannot exceed 99999. Negative values can be used to indicate certain modes. - Homing cycle line number changed to -1, as an indicator. - Fixed a reporting define for the spindle states that was broken by the last merge. --- config.h | 8 ++--- gcode.c | 2 +- limits.c | 92 ++++++++++++++++++++++++------------------------ motion_control.c | 4 +-- motion_control.h | 7 ++-- planner.c | 9 ++--- planner.h | 9 ++--- report.c | 11 +++--- 8 files changed, 72 insertions(+), 70 deletions(-) diff --git a/config.h b/config.h index 88109d3..5b25e29 100644 --- a/config.h +++ b/config.h @@ -28,10 +28,6 @@ #ifndef config_h #define config_h -// Allows GRBL to tranck and report gcode line numbers. Enabling this means that the planning buffer -// goes from 18 or 16 to make room for the additional line number data in the plan_block_t struct -#define USE_LINE_NUMBERS - // Default settings. Used when resetting EEPROM. Change to desired name in defaults.h #define DEFAULTS_SHERLINE_5400 @@ -88,6 +84,10 @@ // parser state depending on user preferences. #define N_STARTUP_LINE 2 // Integer (1-3) +// Allows GRBL to tranck and report gcode line numbers. Enabling this means that the planning buffer +// goes from 18 or 16 to make room for the additional line number data in the plan_block_t struct +// #define USE_LINE_NUMBERS + // Enables a second coolant control pin via the mist coolant g-code command M7 on the Arduino Uno // analog pin 5. Only use this option if you require a second coolant control pin. // NOTE: The M8 flood coolant control pin on analog pin 4 will still be functional regardless. diff --git a/gcode.c b/gcode.c index 450f9af..fc6ad6e 100644 --- a/gcode.c +++ b/gcode.c @@ -101,7 +101,7 @@ uint8_t gc_execute_line(char *line) float target[N_AXIS]; clear_vector(target); // XYZ(ABC) axes parameters. - uint32_t line_number = 0; + int32_t line_number = 0; gc.arc_radius = 0; clear_vector(gc.arc_offset); // IJK Arc offsets are incremental. Value of zero indicates no change. diff --git a/limits.c b/limits.c index 10f0311..fbbfb14 100644 --- a/limits.c +++ b/limits.c @@ -85,11 +85,11 @@ void limits_disable() // limit setting if their limits are constantly triggering after a reset and move their axes. if (sys.state != STATE_ALARM) { if (bit_isfalse(sys.execute,EXEC_ALARM)) { - mc_reset(); // Initiate system kill. - sys.execute |= EXEC_CRIT_EVENT; // Indicate hard limit critical event - } + mc_reset(); // Initiate system kill. + sys.execute |= EXEC_CRIT_EVENT; // Indicate hard limit critical event } - } + } + } #else // OPTIONAL: Software debounce limit pin routine. // Upon limit pin change, enable watchdog timer to create a short delay. ISR(LIMIT_INT_vect) { if (!(WDTCSR & (1<millimeters = 0; block->direction_bits = 0; block->acceleration = SOME_LARGE_VALUE; // Scaled down to maximum acceleration later -#ifdef USE_LINE_NUMBERS - block->line_number = line_number; -#endif + #ifdef USE_LINE_NUMBERS + block->line_number = line_number; + #endif + // Compute and store initial move distance data. // TODO: After this for-loop, we don't touch the stepper algorithm data. Might be a good idea // to try to keep these types of things completely separate from the planner for portability. diff --git a/planner.h b/planner.h index 29b571b..c4bebea 100644 --- a/planner.h +++ b/planner.h @@ -51,9 +51,10 @@ typedef struct { float acceleration; // Axis-limit adjusted line acceleration in (mm/min^2) float millimeters; // The remaining distance for this block to be executed in (mm) // uint8_t max_override; // Maximum override value based on axis speed limits -#ifdef USE_LINE_NUMBERS - uint32_t line_number; -#endif + + #ifdef USE_LINE_NUMBERS + int32_t line_number; + #endif } plan_block_t; @@ -63,7 +64,7 @@ void plan_reset(); // Add a new linear movement to the buffer. target[N_AXIS] is the signed, absolute target position // in millimeters. 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(float *target, float feed_rate, uint8_t invert_feed_rate, uint32_t line_number); +void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate, int32_t line_number); // Called when the current block is no longer needed. Discards the block and makes the memory // availible for new blocks. diff --git a/report.c b/report.c index e17b6a9..13c27a9 100644 --- a/report.c +++ b/report.c @@ -262,9 +262,9 @@ void report_gcode_modes() } switch (gc.spindle_direction) { - case 1 : printPgmString(PSTR(" M3")); break; - case -1 : printPgmString(PSTR(" M4")); break; - case 0 : printPgmString(PSTR(" M5")); break; + case SPINDLE_ENABLE_CW : printPgmString(PSTR(" M3")); break; + case SPINDLE_ENABLE_CCW : printPgmString(PSTR(" M4")); break; + case SPINDLE_DISABLE : printPgmString(PSTR(" M5")); break; } switch (gc.coolant_mode) { @@ -353,9 +353,8 @@ void report_realtime_status() #ifdef USE_LINE_NUMBERS // Report current line number - printPgmString(PSTR(",")); - printPgmString(PSTR("Ln:")); - uint32_t ln=0; + printPgmString(PSTR(",Ln:")); + int32_t ln=0; plan_block_t * pb = plan_get_current_block(); if(pb != NULL) { ln = pb->line_number; From 0a46dfe0b91d58c8b40cf12252ac67516458bf00 Mon Sep 17 00:00:00 2001 From: Robert Grzesek Date: Tue, 25 Feb 2014 12:19:52 -0800 Subject: [PATCH 46/73] Minimal probing cycle working. Supports both G38.2 for error and G38.3 when no errors are desired. --- cpu_map.h | 3 +- gcode.c | 26 +++++++++++++ gcode.h | 6 ++- motion_control.c | 97 ++++++++++++++++++++++++++++++++++++++++++++++++ motion_control.h | 5 +++ report.c | 51 +++++++++++++++++++++++++ report.h | 6 +++ stepper.c | 6 +++ system.c | 5 +++ system.h | 9 +++++ 10 files changed, 211 insertions(+), 3 deletions(-) diff --git a/cpu_map.h b/cpu_map.h index 9adee32..0538bbc 100644 --- a/cpu_map.h +++ b/cpu_map.h @@ -107,10 +107,11 @@ #define PIN_RESET 0 // Uno Analog Pin 0 #define PIN_FEED_HOLD 1 // Uno Analog Pin 1 #define PIN_CYCLE_START 2 // Uno Analog Pin 2 + #define PIN_PROBE 5 // Uno Analog Pin 5 #define PINOUT_INT PCIE1 // Pin change interrupt enable pin #define PINOUT_INT_vect PCINT1_vect #define PINOUT_PCMSK PCMSK1 // Pin change interrupt register - #define PINOUT_MASK ((1<\r\n")); } + +// Prints real-time data. This function grabs a real-time snapshot of the stepper subprogram + // and the actual location of the CNC machine. Users may change the following function to their + // specific needs. It is kept separate from the "normal" report_realtime_status() to allow customization. +void report_realtime_status_probe() +{ + // **Under construction** Bare-bones status report. Provides real-time machine position relative to + // the system power on location (0,0,0) and work coordinate position (G54 and G92 applied). + uint8_t i; + int32_t current_position[N_AXIS]; // Copy current state of the system position variable + memcpy(current_position,sys.position,sizeof(sys.position)); + float print_position[N_AXIS]; + + printPgmString(PSTR("line_number; + } + printInteger(ln); +#endif + + printPgmString(PSTR(">\r\n")); +} diff --git a/report.h b/report.h index 92984df..f05bc42 100644 --- a/report.h +++ b/report.h @@ -36,6 +36,7 @@ #define STATUS_ALARM_LOCK 12 #define STATUS_SOFT_LIMIT_ERROR 13 #define STATUS_OVERFLOW 14 +#define STATUS_PROBE_ERROR 15 // Define Grbl alarm codes. Less than zero to distinguish alarm error from status error. #define ALARM_LIMIT_ERROR -1 @@ -69,6 +70,11 @@ void report_grbl_settings(); // Prints realtime status report void report_realtime_status(); +// Prints realtime position status report at the end of a probe cycle +// This is in leiu of saving the probe position to internal variables like an +// EMC machine +void report_realtime_status_probe(); + // Prints Grbl persistent coordinate parameters void report_gcode_parameters(); diff --git a/stepper.c b/stepper.c index bfe1f74..370d798 100644 --- a/stepper.c +++ b/stepper.c @@ -282,6 +282,12 @@ ISR(TIMER1_COMPA_vect) { // SPINDLE_ENABLE_PORT ^= 1< Date: Wed, 26 Feb 2014 12:10:07 -0700 Subject: [PATCH 47/73] Added grbl planner Matlab simulator for test reference. Updated line number compile-time option. - Added a grbl planner simulation tool that was written in Matlab and Python. It was used to visualize the inner workings of the planner as a program is streamed to it. The simulation assumes that the planner buffer is empty, then filled, and kept filled. This is mainly for users to see how the planner works. - Updated some of the compile-time ifdefs when enabling line numbers. The leaving the un-used line numbers in the function calls eats a non-neglible amount of flash memory. So the new if-defs remove them. --- config.h | 4 +- doc/test/grbl_sim.m | 437 +++++++ doc/test/matlab.gcode | 2362 +++++++++++++++++++++++++++++++++++ doc/test/matlab_convert.py | 270 ++++ doc/test/test.gcode | 2363 ++++++++++++++++++++++++++++++++++++ gcode.c | 38 +- limits.c | 72 +- motion_control.c | 24 +- motion_control.h | 10 +- planner.c | 4 + planner.h | 4 + 11 files changed, 5549 insertions(+), 39 deletions(-) create mode 100644 doc/test/grbl_sim.m create mode 100644 doc/test/matlab.gcode create mode 100755 doc/test/matlab_convert.py create mode 100644 doc/test/test.gcode diff --git a/config.h b/config.h index 5b25e29..55114bf 100644 --- a/config.h +++ b/config.h @@ -86,12 +86,12 @@ // Allows GRBL to tranck and report gcode line numbers. Enabling this means that the planning buffer // goes from 18 or 16 to make room for the additional line number data in the plan_block_t struct -// #define USE_LINE_NUMBERS +// #define USE_LINE_NUMBERS // Disabled by default. Uncomment to enable. // Enables a second coolant control pin via the mist coolant g-code command M7 on the Arduino Uno // analog pin 5. Only use this option if you require a second coolant control pin. // NOTE: The M8 flood coolant control pin on analog pin 4 will still be functional regardless. -// #define ENABLE_M7 // Mist coolant disabled by default. See config.h to enable/disable. +// #define ENABLE_M7 // Disabled by default. Uncomment to enable. // --------------------------------------------------------------------------------------- // ADVANCED CONFIGURATION OPTIONS: diff --git a/doc/test/grbl_sim.m b/doc/test/grbl_sim.m new file mode 100644 index 0000000..3f34d95 --- /dev/null +++ b/doc/test/grbl_sim.m @@ -0,0 +1,437 @@ +% ---------------------------------------------------------------------------------------- +% The MIT License (MIT) +% +% Copyright (c) 2014 Sungeun K. Jeon +% +% Permission is hereby granted, free of charge, to any person obtaining a copy +% of this software and associated documentation files (the "Software"), to deal +% in the Software without restriction, including without limitation the rights +% to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +% copies of the Software, and to permit persons to whom the Software is +% furnished to do so, subject to the following conditions: +% +% The above copyright notice and this permission notice shall be included in +% all copies or substantial portions of the Software. +% +% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +% IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +% FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +% AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +% LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +% OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +% THE SOFTWARE. +% ---------------------------------------------------------------------------------------- + +% This MATLAB script was written for the purpose of being a GRBL planner simulator. This +% simulator is a rough representation of the actual workings of Grbl on the Arduino, but +% was used to hone and proof the actual planner by providing quick visual feedback on its +% functionality when experimented on. This script should be considered for educational +% purposes only. This script requires and executes a pre-parsed g-code file from the +% matlab_convert.py script that is in a specific non-g-code format. + +% There will be two figures plotted. The first is the line motion paths of the complete +% g-code program. The second is a representation of Grbl's planner buffer as new line +% motions are fed to it, plotting the velocity profiles the stepper motors will execute. +% Every time the user inputs an , this feeds the simulator planner one line motion +% block. The left side is the first block in the buffer and the one that will be executed +% by the stepper module first. The right side is the end of the planner buffer, where the +% most recent streamed block is appended onto the planner buffer. Grbl's planner +% optimizes the velocity profiles between the beginning and end of the buffer based on +% the acceleration limits, intended velocity/feedrate, and line motion junction angles +% with their corresponding velocity limits (i.e. junctions with acute angles needs to come +% to a complete stop vs straight junctions can continue through at full speed.) + +% ---------------------------------------------------------------------------------------- + + +% Main function +% NOTE: This is just a way to keep all functions in one place, but all non-global variables +% are cleared as soon as this script completes. +function main() + +% Load pre-parsed gcode moves. +close all; +warning off; +clearvars -global +fid = fopen('matlab.gcode','r'); +gcode = textscan(fid,'%d8%f32%f32%f32%f32'); +nblock = length(gcode{1}); + +% Plot all g-code moves. +figure +line(gcode{3},gcode{4},gcode{5}); +axis equal; +% axis([min(gcode{3}) max(gcode{3}) min(gcode{4}) max(gcode{4}) min(gcode{5}) max(gcode{5})]); +title('G-code programming line motions'); +view(3); + +% Set up figure for planner queue +figure + +% Print help. +disp(''); +disp(' BLUE line indicates completed planner blocks that require no recalculation.'); +disp(' RED line indicates planner blocks that have been recalculated.'); +disp(' GREEN line indicates the location of the BPLANNED pointer. Always a recalculated block.'); +disp(' BLACK dotted-line and ''x'' indicates block nominal speed and max junction velocity, respectively.'); +disp(' CYAN ''.'' indicates block initial entry speed.'); + +% Define Grbl settings. +BUFFER_SIZE = 18; % Number of planner blocks in its ring buffer. +steps_per_mm = 200; +seekrate = 2500; % mm/min +acceleration = [100 100 100]; % mm/sec^2 [ X Y Z ] axes +junction_deviation = 0.1; % mm. See Grbl documentation on this parameter. +inch_2_mm = 25.4; +ACCELERATION_TICKS_PER_SECOND = 100; + +gcode{2} = gcode{2}; +gcode{2} = inch_2_mm*gcode{2}; +gcode{3} = inch_2_mm*gcode{3}; +gcode{4} = inch_2_mm*gcode{4}; +gcode{5} = inch_2_mm*gcode{5}; + +% Initialize blocks +block.steps = []; +block.step_event_count = []; +block.delta_mm = []; +block.millimeters = []; +block.acceleration = []; +block.speed = []; +block.nominal_speed = []; +block.max_entry_speed = []; +block.entry_speed = []; +block.recalculate_flag = false; +for i = 2:BUFFER_SIZE + block(i) = block(1); +end + +% Initialize planner +position = [0 0 0]; +prev_unit_vec = [0 0 0]; +previous_nominal_speed = 0; +pos = 0; + +% BHEAD and BTAIL act as pointers to the block head and tail. +% BPLANNED acts as a pointer of the location of the end of a completed/optimized plan. +bhead = 1; +btail = 1; +bplanned = 1; + +global block bhead btail bplanned nind acceleration BUFFER_SIZE pos ACCELERATION_TICKS_PER_SECOND + +% Main loop. Simulates plan_buffer_line(). All of the precalculations for the newest incoming +% block occurs here. Anything independent of the planner changes. +for i = 1:nblock + + target = round([gcode{3}(i) gcode{4}(i) gcode{5}(i)].*steps_per_mm); + if gcode{1}(i) == 1 + feedrate = gcode{2}(i); + else + feedrate = seekrate; + end + + nind = next_block_index(bhead); + if nind == btail + % Simulate a constantly full buffer. Move buffer tail. + bind = next_block_index(btail); + % Push planned pointer if encountered. Prevents it from looping back around the ring buffer. + if btail == bplanned; bplanned = bind; end + btail = bind; + end + + block(bhead).steps = abs(target-position); + block(bhead).step_event_count = max(block(bhead).steps); + + % Bail if this is a zero-length block + if block(bhead).step_event_count == 0 + disp(['Zero-length block in line ',int2str(i)]); + else + + % Compute path vector in terms of absolute step target and current positions + delta_mm = single((target-position)./steps_per_mm); + block(bhead).millimeters = single(norm(delta_mm)); + inverse_millimeters = single(1/block(bhead).millimeters); + + % Compute path unit vector + unit_vec = delta_mm/block(bhead).millimeters; + + % Calculate speed in mm/minute for each axis + inverse_minute = single(feedrate * inverse_millimeters); + block(bhead).speed = delta_mm*inverse_minute; + block(bhead).nominal_speed = block(bhead).millimeters*inverse_minute; + + % Calculate block acceleration. Operates on absolute value of unit vector. + [max_acc,ind] = max(abs(unit_vec)./acceleration); % Determine limiting acceleration + block(bhead).acceleration = acceleration(ind)/abs(unit_vec(ind)); + + % Compute maximum junction speed + block(bhead).max_entry_speed = 0.0; + if previous_nominal_speed > 0.0 + cos_theta = dot(-previous_unit_vec,unit_vec); + if (cos_theta < 0.95) + block(bhead).max_entry_speed = min([block(bhead).nominal_speed,previous_nominal_speed]); + if (cos_theta > -0.95) + sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); + block(bhead).max_entry_speed = min([block(bhead).max_entry_speed,sqrt(block(bhead).acceleration*3600*junction_deviation*sin_theta_d2/(1.0-sin_theta_d2))]); + end + end + end + + block(bhead).entry_speed = 0; % Just initialize. Set accurately in the replanning function. + block(bhead).recalculate_flag = true; % Plotting flag to indicate this block has been updated. + + previous_unit_vec = unit_vec; + previous_nominal_speed = block(bhead).nominal_speed; + position = target; + + bhead = nind; % Block complete. Push buffer pointer. + planner_recalculate(); + + plot_buffer_velocities(); + end +end +return + +% Computes the next block index in the planner ring buffer +function block_index = next_block_index(block_index) +global BUFFER_SIZE + block_index = block_index + 1; + if block_index > BUFFER_SIZE + block_index = 1; + end +return + +% Computes the previous block index in the planner ring buffer +function block_index = prev_block_index(block_index) +global BUFFER_SIZE + block_index = block_index-1; + if block_index < 1 + block_index = BUFFER_SIZE; + end +return + + +% Planner recalculate function. The magic happens here. +function planner_recalculate(block) + + global block bhead btail bplanned acceleration + + bind = prev_block_index(bhead); + if bind == bplanned; return; end % Bail, if only one block in buffer. Can't be operated on. + + % Reverse Pass: Coarsely maximize all possible deceleration curves back-planning from the last + % block in buffer. Cease planning when the last optimal planned or tail pointer is reached. + % NOTE: Forward pass will later refine and correct the reverse pass to create an optimal plan. + next = []; + curr = bind; % Last block in buffer. + + % Calculate maximum entry speed for last block in buffer, where the exit speed is always zero. + block(curr).entry_speed = min([block(curr).max_entry_speed,sqrt(2*block(curr).acceleration*60*60*block(curr).millimeters)]); + + bind = prev_block_index(bind); % Btail or second to last block + if (bind == bplanned) + % Only two plannable blocks in buffer. Reverse pass complete. + % Check if the first block is the tail. If so, notify stepper module to update its current parameters. + % if bind == btail; update_tail_block; end + else + % Three or more plannable blocks in buffer. Loop it. + while bind ~= bplanned % Loop until bplanned point hits. Replans to last plan point. + next = curr; + curr = bind; + bind = prev_block_index( bind ); % Previous block pointer. + + % Check if the first block is the tail. If so, notify stepper module to update its current parameters. + % if bind == btail; update_tail_block; end + + % Compute maximum entry speed decelerating over the current block from its exit speed. + if block(curr).entry_speed ~= block(curr).max_entry_speed + block(curr).recalculate_flag = true; % Plotting flag to indicate this block has been updated. + block(curr).entry_speed = min([ block(curr).max_entry_speed,... + sqrt(block(next).entry_speed^2 + 2*block(curr).acceleration*60*60*block(curr).millimeters)]); + end + + end + end + + % For two blocks, reverse pass is skipped, but forward pass plans second block entry speed + % onward. This prevents the first, or the potentially executing block, from being over-written. + % NOTE: Can never be bhead, since bsafe is always in active buffer. + next = bplanned; + bind = next_block_index(bplanned); % Start at bplanned + while bind ~= bhead + curr = next; + next = bind; + + % An acceleration block is always an optimally planned block since it starts from the first + % block's current speed or a maximum junction speed. Compute accelerations from this block + % and update the next block's entry speed. + if (block(curr).entry_speed < block(next).entry_speed) + % Once speed is set by forward planner, the plan for this block is finished and optimal. + % Increment the planner pointer forward one block. + + entry_speed = sqrt(block(curr).entry_speed^2 + 2*block(curr).acceleration*60*60*block(curr).millimeters); + if (block(next).entry_speed > entry_speed) + block(next).entry_speed = entry_speed; + bplanned = bind; + end + + end + + % Check if the next block entry speed is at max_entry_speed. If so, move the planned pointer, since + % this entry speed cannot be improved anymore and all prior blocks have been completed and optimally planned. + if block(next).entry_speed == block(next).max_entry_speed + bplanned = bind; + end + + % Recalculate trapezoid can be installed here, since it scans through all of the plannable blocks. + % NOTE: Eventually this will only be computed when being executed. + + bind = next_block_index( bind ); + + end + +return + +% ---------------------------------------------------------------------------------------- +% PLOTTING FUNCTIONS + +% Plots the entire buffer plan into a MATLAB figure to visual the plan. +% BLUE line indicates completed planner blocks that require no recalculation. +% RED line indicates planner blocks that have been recalculated. +% GREEN line indicates the location of the BPLANNED pointer. Always a recalculated block. +% BLACK dotted-line and 'x' indicates block nominal speed and max junction velocity, respectively. +% CYAN '.' indicates block initial entry speed. +function plot_buffer_velocities() + global block bhead btail bplanned acceleration pos ACCELERATION_TICKS_PER_SECOND + bind = btail; + curr = []; + next = []; + + pos_initial = 0; + pos = 0; + while bind ~= bhead + curr = next; + next = bind; + hold on; + if ~isempty(curr) + accel_d = estimate_acceleration_distance(block(curr).entry_speed, block(curr).nominal_speed, block(curr).acceleration*60*60); + decel_d = estimate_acceleration_distance(block(curr).nominal_speed, block(next).entry_speed,-block(curr).acceleration*60*60); + plateau_d = block(curr).millimeters-accel_d-decel_d; + if plateau_d < 0 + accel_d = intersection_distance(block(curr).entry_speed, block(next).entry_speed, block(curr).acceleration*60*60, block(curr).millimeters); + if accel_d < 0 + accel_d = 0; + elseif accel_d > block(curr).millimeters + accel_d = block(curr).millimeters; + end + plateau_d = 0; + end + color = 'b'; + if (block(curr).recalculate_flag || block(next).recalculate_flag) + block(curr).recalculate_flag = false; + color = 'r'; + end + if bplanned == curr + color = 'g'; + end + + plot_trap(pos,block(curr).entry_speed,block(next).entry_speed,block(curr).nominal_speed,block(curr).acceleration,accel_d,plateau_d,block(curr).millimeters,color) + plot([pos pos+block(curr).millimeters],block(curr).nominal_speed*[1 1],'k:') % BLACK dotted indicates + plot(pos,block(curr).max_entry_speed,'kx') + + pos = pos + block(curr).millimeters; + plot(pos,block(next).entry_speed,'c.'); + end + bind = next_block_index( bind ); + end + + accel_d = estimate_acceleration_distance(block(next).entry_speed, block(next).nominal_speed, block(next).acceleration*60*60); + decel_d = estimate_acceleration_distance(block(next).nominal_speed, 0, -block(next).acceleration*60*60); + plateau_d = block(next).millimeters-accel_d-decel_d; + if plateau_d < 0 + accel_d = intersection_distance(block(next).entry_speed, 0, block(next).acceleration*60*60, block(next).millimeters); + if accel_d < 0 + accel_d = 0; + elseif accel_d > block(next).millimeters + accel_d = block(next).millimeters; + end + plateau_d = 0; + end + block(next).recalculate_flag = false; + color = 'r'; + if bplanned == next + color= 'g'; + end + + plot_trap(pos,block(next).entry_speed,0,block(next).nominal_speed,block(next).acceleration,accel_d,plateau_d,block(next).millimeters,color) + plot([pos pos+block(next).millimeters],block(next).nominal_speed*[1 1],'k:') + plot(pos,block(next).max_entry_speed,'kx') + + plot(pos,block(next).entry_speed,'.'); + pos = pos + block(next).millimeters; + plot(pos,0,'rx'); + xlabel('mm'); + ylabel('mm/sec'); + xlim([pos_initial pos]) + title('Planner buffer optimized velocity profile'); + pause(); + hold off; + + plot(pos,0) +return + + +function d_a = estimate_acceleration_distance(initial_rate, target_rate, acceleration,rate_delta) + d_a = (target_rate*target_rate-initial_rate*initial_rate)/(2*acceleration); +return + +function d_i = intersection_distance(initial_rate, final_rate, acceleration, distance, rate_delta) + d_i = (2*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/(4*acceleration); +return + + +% Simply plots the ac/de-celeration curves and plateaus of a trapezoid. +function plot_trap(pos,initial_rate,final_rate,rate,accel,accel_d,plateau_d,millimeters,color) + + dx = 1.0; % Line segment length + linex = [pos]; liney = [initial_rate]; + + % Acceleration + np = floor(accel_d/dx); + if np + v = initial_rate; + for i = 1:np + v = sqrt(v^2+2*accel*60*60*dx); + linex = [linex pos+i*dx]; + liney = [liney v]; + end + end + + % Plateau + v = sqrt(initial_rate^2 + 2*accel*60*60*accel_d); + if v < rate + rate = v; + end + linex = [linex pos+[accel_d accel_d+plateau_d]]; + liney = [liney [rate rate]]; + + % Deceleration + np = floor((millimeters-accel_d-plateau_d)/dx); + if np + v = rate; + for i = 1:np + v = sqrt(v^2-2*accel*60*60*dx); + linex = [linex pos+i*dx+accel_d+plateau_d]; + liney = [liney v]; + end + end + + linex = [linex pos+millimeters]; + liney = [ liney final_rate]; + plot(linex,liney,color); + +return + + + diff --git a/doc/test/matlab.gcode b/doc/test/matlab.gcode new file mode 100644 index 0000000..8ad9cdc --- /dev/null +++ b/doc/test/matlab.gcode @@ -0,0 +1,2362 @@ +0 0.0 0.0 0.0 0.0 +0 0.0 0.0 0.0 0.0 +0 0.0 0.0 0.0 0.0 +0 0.0 0.0 0.0 6.0 +0 0.0 37.56 12.33 6.0 +1 300.0 37.56 12.33 -1.0 +1 300.0 37.56 0.88 -1.0 +1 300.0 49.01 0.88 -1.0 +1 300.0 49.01 12.33 -1.0 +1 300.0 37.56 12.33 -1.0 +0 300.0 37.56 12.33 6.0 +0 300.0 37.56 0.88 6.0 +1 300.0 37.56 0.88 -1.0 +1 300.0 37.56 -10.57 -1.0 +1 300.0 49.01 -10.57 -1.0 +1 300.0 49.01 0.88 -1.0 +1 300.0 37.56 0.88 -1.0 +0 300.0 37.56 0.88 6.0 +0 300.0 49.01 12.33 6.0 +1 300.0 49.01 12.33 -1.0 +1 300.0 52.08 15.01 -1.0 +0 300.0 52.08 15.01 6.0 +0 300.0 49.01 0.88 6.0 +1 300.0 49.01 0.88 -1.0 +1 300.0 52.08 6.21 -1.0 +1 300.0 52.08 15.01 -1.0 +1 300.0 43.29 15.01 -1.0 +1 300.0 37.56 12.33 -1.0 +0 300.0 37.56 12.33 6.0 +0 300.0 49.01 -10.57 6.0 +1 300.0 49.01 -10.57 -1.0 +1 300.0 52.08 -2.58 -1.0 +1 300.0 52.08 6.21 -1.0 +1 300.0 49.01 0.88 -1.0 +0 300.0 49.01 0.88 6.0 +0 300.0 49.01 0.88 20.0 +0 300.0 0.0 0.0 20.0 +1 300.0 -7.1 -39.88 20.0 +1 300.0 -10.3 -38.12 20.0 +1 300.0 -10.7 -37.9 20.0 +1 300.0 -11.1 -37.68 20.0 +1 300.0 -11.89 -37.23 20.0 +1 300.0 -13.45 -36.36 20.0 +1 300.0 -13.82 -36.14 20.0 +1 300.0 -14.2 -35.92 20.0 +1 300.0 -14.92 -35.48 20.0 +1 300.0 -16.29 -34.63 20.0 +1 300.0 -16.64 -34.4 20.0 +1 300.0 -16.97 -34.16 20.0 +1 300.0 -17.61 -33.71 20.0 +1 300.0 -17.91 -33.49 20.0 +1 300.0 -18.2 -33.26 20.0 +1 300.0 -18.74 -32.82 20.0 +1 300.0 -18.99 -32.6 20.0 +1 300.0 -19.23 -32.39 20.0 +1 300.0 -19.45 -32.17 20.0 +1 300.0 -19.65 -31.96 20.0 +1 300.0 -19.84 -31.75 20.0 +1 300.0 -20.02 -31.54 20.0 +1 300.0 -20.18 -31.34 20.0 +1 300.0 -20.32 -31.13 20.0 +1 300.0 -20.45 -30.93 20.0 +1 300.0 -20.56 -30.73 20.0 +1 300.0 -20.65 -30.54 20.0 +1 300.0 -20.73 -30.34 20.0 +1 300.0 -20.79 -30.15 20.0 +1 300.0 -20.84 -29.96 20.0 +1 300.0 -20.87 -29.78 20.0 +1 300.0 -20.88 -29.59 20.0 +1 300.0 -20.88 -29.41 20.0 +1 300.0 -20.85 -29.24 20.0 +1 300.0 -20.82 -29.06 20.0 +1 300.0 -20.76 -28.89 20.0 +1 300.0 -20.7 -28.72 20.0 +1 300.0 -20.61 -28.56 20.0 +1 300.0 -20.51 -28.4 20.0 +1 300.0 -20.39 -28.24 20.0 +1 300.0 -20.26 -28.08 20.0 +1 300.0 -20.12 -27.93 20.0 +1 300.0 -19.96 -27.78 20.0 +1 300.0 -19.78 -27.64 20.0 +1 300.0 -19.59 -27.5 20.0 +1 300.0 -19.39 -27.36 20.0 +1 300.0 -19.18 -27.23 20.0 +1 300.0 -18.95 -27.09 20.0 +1 300.0 -18.71 -26.97 20.0 +1 300.0 -18.46 -26.84 20.0 +1 300.0 -18.2 -26.73 20.0 +1 300.0 -17.92 -26.61 20.0 +1 300.0 -17.64 -26.5 20.0 +1 300.0 -17.35 -26.39 20.0 +1 300.0 -16.74 -26.18 20.0 +1 300.0 -16.42 -26.09 20.0 +1 300.0 -16.09 -25.99 20.0 +1 300.0 -15.76 -25.9 20.0 +1 300.0 -15.43 -25.82 20.0 +1 300.0 -15.08 -25.74 20.0 +1 300.0 -14.74 -25.66 20.0 +1 300.0 -14.03 -25.52 20.0 +1 300.0 -13.67 -25.45 20.0 +1 300.0 -13.31 -25.39 20.0 +1 300.0 -12.95 -25.33 20.0 +1 300.0 -12.59 -25.28 20.0 +1 300.0 -12.23 -25.23 20.0 +1 300.0 -11.87 -25.18 20.0 +1 300.0 -11.51 -25.14 20.0 +1 300.0 -11.15 -25.1 20.0 +1 300.0 -10.82 -25.07 20.0 +1 300.0 -10.5 -25.04 20.0 +1 300.0 -10.17 -25.02 20.0 +1 300.0 -9.85 -25.0 20.0 +1 300.0 -9.54 -24.98 20.0 +1 300.0 -9.23 -24.96 20.0 +1 300.0 -8.92 -24.95 20.0 +1 300.0 -8.63 -24.94 20.0 +1 300.0 -8.33 -24.94 20.0 +1 300.0 -8.05 -24.94 20.0 +1 300.0 -7.77 -24.94 20.0 +1 300.0 -7.5 -24.95 20.0 +1 300.0 -7.24 -24.95 20.0 +1 300.0 -6.99 -24.97 20.0 +1 300.0 -6.75 -24.98 20.0 +1 300.0 -6.52 -25.0 20.0 +1 300.0 -6.29 -25.02 20.0 +1 300.0 -6.08 -25.05 20.0 +1 300.0 -5.88 -25.07 20.0 +1 300.0 -5.69 -25.11 20.0 +1 300.0 -5.52 -25.14 20.0 +1 300.0 -5.35 -25.18 20.0 +1 300.0 -5.2 -25.22 20.0 +1 300.0 -5.06 -25.26 20.0 +1 300.0 -4.93 -25.31 20.0 +1 300.0 -4.81 -25.36 20.0 +1 300.0 -4.71 -25.41 20.0 +1 300.0 -4.62 -25.46 20.0 +1 300.0 -4.55 -25.52 20.0 +1 300.0 -4.49 -25.58 20.0 +1 300.0 -4.44 -25.64 20.0 +1 300.0 -4.41 -25.71 20.0 +1 300.0 -4.39 -25.77 20.0 +1 300.0 -4.38 -25.84 20.0 +1 300.0 -4.39 -25.92 20.0 +1 300.0 -4.42 -25.99 20.0 +1 300.0 -4.45 -26.07 20.0 +1 300.0 -4.51 -26.15 20.0 +1 300.0 -4.57 -26.23 20.0 +1 300.0 -4.65 -26.32 20.0 +1 300.0 -4.75 -26.41 20.0 +1 300.0 -4.86 -26.5 20.0 +1 300.0 -4.98 -26.59 20.0 +1 300.0 -5.12 -26.68 20.0 +1 300.0 -5.27 -26.78 20.0 +1 300.0 -5.43 -26.87 20.0 +1 300.0 -5.8 -27.07 20.0 +1 300.0 -6.0 -27.18 20.0 +1 300.0 -6.22 -27.28 20.0 +1 300.0 -6.68 -27.5 20.0 +1 300.0 -7.75 -27.94 20.0 +1 300.0 -8.05 -28.06 20.0 +1 300.0 -8.35 -28.17 20.0 +1 300.0 -8.99 -28.41 20.0 +1 300.0 -10.36 -28.89 20.0 +1 300.0 -10.75 -29.02 20.0 +1 300.0 -11.15 -29.16 20.0 +1 300.0 -11.97 -29.43 20.0 +1 300.0 -13.66 -29.97 20.0 +1 300.0 -17.11 -31.07 20.0 +1 300.0 -17.54 -31.2 20.0 +1 300.0 -17.96 -31.34 20.0 +1 300.0 -18.79 -31.6 20.0 +1 300.0 -20.37 -32.13 20.0 +1 300.0 -20.75 -32.26 20.0 +1 300.0 -21.11 -32.38 20.0 +1 300.0 -21.82 -32.63 20.0 +1 300.0 -23.09 -33.11 20.0 +1 300.0 -23.38 -33.23 20.0 +1 300.0 -23.66 -33.34 20.0 +1 300.0 -24.16 -33.56 20.0 +1 300.0 -24.4 -33.67 20.0 +1 300.0 -24.61 -33.77 20.0 +1 300.0 -25.0 -33.97 20.0 +1 300.0 -25.18 -34.07 20.0 +1 300.0 -25.33 -34.17 20.0 +1 300.0 -25.47 -34.26 20.0 +1 300.0 -25.59 -34.35 20.0 +1 300.0 -25.7 -34.44 20.0 +1 300.0 -25.79 -34.52 20.0 +1 300.0 -25.86 -34.6 20.0 +1 300.0 -25.92 -34.68 20.0 +1 300.0 -25.96 -34.75 20.0 +1 300.0 -25.98 -34.83 20.0 +1 300.0 -25.99 -34.9 20.0 +1 300.0 -25.98 -34.96 20.0 +1 300.0 -25.95 -35.02 20.0 +1 300.0 -25.9 -35.08 20.0 +1 300.0 -25.84 -35.14 20.0 +1 300.0 -25.76 -35.19 20.0 +1 300.0 -25.67 -35.24 20.0 +1 300.0 -25.56 -35.28 20.0 +1 300.0 -25.43 -35.33 20.0 +1 300.0 -25.29 -35.36 20.0 +1 300.0 -25.13 -35.4 20.0 +1 300.0 -24.96 -35.43 20.0 +1 300.0 -24.78 -35.45 20.0 +1 300.0 -24.58 -35.48 20.0 +1 300.0 -24.37 -35.5 20.0 +1 300.0 -24.14 -35.51 20.0 +1 300.0 -23.91 -35.52 20.0 +1 300.0 -23.66 -35.53 20.0 +1 300.0 -23.4 -35.53 20.0 +1 300.0 -23.13 -35.53 20.0 +1 300.0 -22.85 -35.53 20.0 +1 300.0 -22.56 -35.52 20.0 +1 300.0 -22.26 -35.51 20.0 +1 300.0 -21.95 -35.49 20.0 +1 300.0 -21.64 -35.47 20.0 +1 300.0 -21.31 -35.45 20.0 +1 300.0 -20.98 -35.42 20.0 +1 300.0 -20.64 -35.38 20.0 +1 300.0 -20.3 -35.35 20.0 +1 300.0 -19.95 -35.31 20.0 +1 300.0 -19.59 -35.26 20.0 +1 300.0 -19.24 -35.21 20.0 +1 300.0 -18.88 -35.16 20.0 +1 300.0 -18.51 -35.1 20.0 +1 300.0 -18.14 -35.04 20.0 +1 300.0 -17.78 -34.97 20.0 +1 300.0 -17.04 -34.83 20.0 +1 300.0 -16.67 -34.75 20.0 +1 300.0 -16.31 -34.67 20.0 +1 300.0 -15.58 -34.49 20.0 +1 300.0 -15.22 -34.4 20.0 +1 300.0 -14.87 -34.3 20.0 +1 300.0 -14.17 -34.09 20.0 +1 300.0 -13.83 -33.98 20.0 +1 300.0 -13.5 -33.86 20.0 +1 300.0 -13.18 -33.74 20.0 +1 300.0 -12.86 -33.62 20.0 +1 300.0 -12.55 -33.49 20.0 +1 300.0 -12.25 -33.36 20.0 +1 300.0 -11.68 -33.08 20.0 +1 300.0 -11.4 -32.94 20.0 +1 300.0 -11.14 -32.79 20.0 +1 300.0 -10.9 -32.64 20.0 +1 300.0 -10.66 -32.48 20.0 +1 300.0 -10.44 -32.33 20.0 +1 300.0 -10.22 -32.16 20.0 +1 300.0 -10.03 -32.0 20.0 +1 300.0 -9.84 -31.83 20.0 +1 300.0 -9.67 -31.65 20.0 +1 300.0 -9.51 -31.48 20.0 +1 300.0 -9.37 -31.3 20.0 +1 300.0 -9.24 -31.11 20.0 +1 300.0 -9.13 -30.92 20.0 +1 300.0 -9.03 -30.73 20.0 +1 300.0 -8.95 -30.54 20.0 +1 300.0 -8.88 -30.34 20.0 +1 300.0 -8.84 -30.14 20.0 +1 300.0 -8.8 -29.93 20.0 +1 300.0 -8.78 -29.73 20.0 +1 300.0 -8.78 -29.52 20.0 +1 300.0 -8.79 -29.3 20.0 +1 300.0 -8.82 -29.09 20.0 +1 300.0 -8.87 -28.87 20.0 +1 300.0 -8.93 -28.64 20.0 +1 300.0 -9.01 -28.43 20.0 +1 300.0 -9.09 -28.22 20.0 +1 300.0 -9.19 -28.01 20.0 +1 300.0 -9.31 -27.79 20.0 +1 300.0 -9.44 -27.57 20.0 +1 300.0 -9.58 -27.35 20.0 +1 300.0 -9.9 -26.9 20.0 +1 300.0 -10.08 -26.68 20.0 +1 300.0 -10.27 -26.45 20.0 +1 300.0 -10.69 -25.98 20.0 +1 300.0 -10.92 -25.75 20.0 +1 300.0 -11.16 -25.51 20.0 +1 300.0 -11.68 -25.03 20.0 +1 300.0 -11.95 -24.79 20.0 +1 300.0 -12.23 -24.55 20.0 +1 300.0 -12.82 -24.06 20.0 +1 300.0 -14.11 -23.07 20.0 +1 300.0 -17.0 -21.04 20.0 +1 300.0 -17.38 -20.78 20.0 +1 300.0 -17.76 -20.52 20.0 +1 300.0 -18.53 -20.01 20.0 +1 300.0 -20.08 -18.98 20.0 +1 300.0 -20.46 -18.72 20.0 +1 300.0 -20.84 -18.46 20.0 +1 300.0 -21.6 -17.95 20.0 +1 300.0 -23.07 -16.93 20.0 +1 300.0 -23.43 -16.67 20.0 +1 300.0 -23.78 -16.42 20.0 +1 300.0 -24.45 -15.91 20.0 +1 300.0 -25.72 -14.91 20.0 +1 300.0 -26.01 -14.66 20.0 +1 300.0 -26.3 -14.42 20.0 +1 300.0 -26.83 -13.93 20.0 +1 300.0 -27.09 -13.69 20.0 +1 300.0 -27.33 -13.45 20.0 +1 300.0 -27.78 -12.97 20.0 +1 300.0 -28.0 -12.71 20.0 +1 300.0 -28.21 -12.46 20.0 +1 300.0 -28.4 -12.21 20.0 +1 300.0 -28.58 -11.96 20.0 +1 300.0 -28.75 -11.71 20.0 +1 300.0 -28.89 -11.47 20.0 +1 300.0 -29.03 -11.23 20.0 +1 300.0 -29.14 -10.99 20.0 +1 300.0 -29.24 -10.75 20.0 +1 300.0 -29.32 -10.51 20.0 +1 300.0 -29.39 -10.28 20.0 +1 300.0 -29.44 -10.05 20.0 +1 300.0 -29.48 -9.82 20.0 +1 300.0 -29.49 -9.6 20.0 +1 300.0 -29.49 -9.38 20.0 +1 300.0 -29.48 -9.16 20.0 +1 300.0 -29.44 -8.95 20.0 +1 300.0 -29.39 -8.73 20.0 +1 300.0 -29.32 -8.53 20.0 +1 300.0 -29.24 -8.32 20.0 +1 300.0 -29.14 -8.12 20.0 +1 300.0 -29.03 -7.92 20.0 +1 300.0 -28.9 -7.72 20.0 +1 300.0 -28.75 -7.53 20.0 +1 300.0 -28.59 -7.34 20.0 +1 300.0 -28.42 -7.16 20.0 +1 300.0 -28.23 -6.97 20.0 +1 300.0 -28.02 -6.8 20.0 +1 300.0 -27.8 -6.62 20.0 +1 300.0 -27.57 -6.45 20.0 +1 300.0 -27.07 -6.12 20.0 +1 300.0 -26.8 -5.96 20.0 +1 300.0 -26.52 -5.8 20.0 +1 300.0 -25.93 -5.5 20.0 +1 300.0 -25.62 -5.36 20.0 +1 300.0 -25.3 -5.22 20.0 +1 300.0 -24.63 -4.95 20.0 +1 300.0 -24.28 -4.82 20.0 +1 300.0 -23.93 -4.69 20.0 +1 300.0 -23.21 -4.46 20.0 +1 300.0 -22.84 -4.34 20.0 +1 300.0 -22.46 -4.24 20.0 +1 300.0 -21.71 -4.03 20.0 +1 300.0 -21.32 -3.94 20.0 +1 300.0 -20.94 -3.84 20.0 +1 300.0 -20.17 -3.67 20.0 +1 300.0 -19.79 -3.59 20.0 +1 300.0 -19.4 -3.52 20.0 +1 300.0 -18.64 -3.38 20.0 +1 300.0 -18.27 -3.31 20.0 +1 300.0 -17.89 -3.25 20.0 +1 300.0 -17.53 -3.2 20.0 +1 300.0 -17.16 -3.15 20.0 +1 300.0 -16.81 -3.1 20.0 +1 300.0 -16.46 -3.06 20.0 +1 300.0 -16.12 -3.02 20.0 +1 300.0 -15.78 -2.98 20.0 +1 300.0 -15.48 -2.95 20.0 +1 300.0 -15.18 -2.92 20.0 +1 300.0 -14.89 -2.9 20.0 +1 300.0 -14.61 -2.88 20.0 +1 300.0 -14.34 -2.87 20.0 +1 300.0 -14.08 -2.85 20.0 +1 300.0 -13.83 -2.85 20.0 +1 300.0 -13.59 -2.84 20.0 +1 300.0 -13.36 -2.84 20.0 +1 300.0 -13.14 -2.84 20.0 +1 300.0 -12.93 -2.84 20.0 +1 300.0 -12.73 -2.85 20.0 +1 300.0 -12.55 -2.86 20.0 +1 300.0 -12.38 -2.87 20.0 +1 300.0 -12.22 -2.89 20.0 +1 300.0 -12.07 -2.91 20.0 +1 300.0 -11.94 -2.93 20.0 +1 300.0 -11.82 -2.96 20.0 +1 300.0 -11.72 -2.98 20.0 +1 300.0 -11.63 -3.01 20.0 +1 300.0 -11.55 -3.05 20.0 +1 300.0 -11.48 -3.08 20.0 +1 300.0 -11.44 -3.12 20.0 +1 300.0 -11.4 -3.17 20.0 +1 300.0 -11.38 -3.21 20.0 +1 300.0 -11.37 -3.26 20.0 +1 300.0 -11.38 -3.31 20.0 +1 300.0 -11.4 -3.36 20.0 +1 300.0 -11.44 -3.42 20.0 +1 300.0 -11.49 -3.47 20.0 +1 300.0 -11.56 -3.53 20.0 +1 300.0 -11.64 -3.59 20.0 +1 300.0 -11.73 -3.66 20.0 +1 300.0 -11.84 -3.72 20.0 +1 300.0 -11.96 -3.79 20.0 +1 300.0 -12.09 -3.86 20.0 +1 300.0 -12.24 -3.94 20.0 +1 300.0 -12.4 -4.01 20.0 +1 300.0 -12.77 -4.17 20.0 +1 300.0 -12.97 -4.25 20.0 +1 300.0 -13.18 -4.33 20.0 +1 300.0 -13.64 -4.5 20.0 +1 300.0 -14.69 -4.85 20.0 +1 300.0 -14.98 -4.94 20.0 +1 300.0 -15.28 -5.04 20.0 +1 300.0 -15.9 -5.22 20.0 +1 300.0 -17.23 -5.62 20.0 +1 300.0 -20.15 -6.43 20.0 +1 300.0 -20.52 -6.53 20.0 +1 300.0 -20.89 -6.63 20.0 +1 300.0 -21.63 -6.84 20.0 +1 300.0 -23.11 -7.24 20.0 +1 300.0 -25.89 -8.03 20.0 +1 300.0 -26.22 -8.12 20.0 +1 300.0 -26.54 -8.22 20.0 +1 300.0 -27.14 -8.4 20.0 +1 300.0 -28.26 -8.76 20.0 +1 300.0 -28.51 -8.85 20.0 +1 300.0 -28.76 -8.93 20.0 +1 300.0 -29.21 -9.1 20.0 +1 300.0 -29.43 -9.18 20.0 +1 300.0 -29.62 -9.25 20.0 +1 300.0 -29.99 -9.4 20.0 +1 300.0 -30.15 -9.48 20.0 +1 300.0 -30.3 -9.55 20.0 +1 300.0 -30.43 -9.62 20.0 +1 300.0 -30.56 -9.68 20.0 +1 300.0 -30.67 -9.75 20.0 +1 300.0 -30.77 -9.81 20.0 +1 300.0 -30.85 -9.87 20.0 +1 300.0 -30.92 -9.93 20.0 +1 300.0 -30.97 -9.99 20.0 +1 300.0 -31.02 -10.04 20.0 +1 300.0 -31.04 -10.09 20.0 +1 300.0 -31.06 -10.14 20.0 +1 300.0 -31.06 -10.19 20.0 +1 300.0 -31.04 -10.23 20.0 +1 300.0 -31.02 -10.27 20.0 +1 300.0 -30.97 -10.31 20.0 +1 300.0 -30.92 -10.35 20.0 +1 300.0 -30.85 -10.38 20.0 +1 300.0 -30.76 -10.41 20.0 +1 300.0 -30.67 -10.44 20.0 +1 300.0 -30.56 -10.47 20.0 +1 300.0 -30.43 -10.49 20.0 +1 300.0 -30.3 -10.51 20.0 +1 300.0 -30.15 -10.53 20.0 +1 300.0 -29.98 -10.54 20.0 +1 300.0 -29.81 -10.55 20.0 +1 300.0 -29.62 -10.56 20.0 +1 300.0 -29.42 -10.56 20.0 +1 300.0 -29.21 -10.56 20.0 +1 300.0 -28.99 -10.56 20.0 +1 300.0 -28.76 -10.56 20.0 +1 300.0 -28.52 -10.55 20.0 +1 300.0 -28.24 -10.54 20.0 +1 300.0 -27.95 -10.52 20.0 +1 300.0 -27.66 -10.5 20.0 +1 300.0 -27.35 -10.48 20.0 +1 300.0 -27.03 -10.45 20.0 +1 300.0 -26.7 -10.42 20.0 +1 300.0 -26.37 -10.38 20.0 +1 300.0 -26.02 -10.34 20.0 +1 300.0 -25.67 -10.3 20.0 +1 300.0 -25.31 -10.25 20.0 +1 300.0 -24.57 -10.15 20.0 +1 300.0 -24.19 -10.09 20.0 +1 300.0 -23.81 -10.02 20.0 +1 300.0 -23.03 -9.88 20.0 +1 300.0 -22.64 -9.81 20.0 +1 300.0 -22.25 -9.73 20.0 +1 300.0 -21.45 -9.56 20.0 +1 300.0 -21.06 -9.46 20.0 +1 300.0 -20.66 -9.37 20.0 +1 300.0 -19.88 -9.16 20.0 +1 300.0 -19.49 -9.05 20.0 +1 300.0 -19.1 -8.94 20.0 +1 300.0 -18.34 -8.7 20.0 +1 300.0 -17.97 -8.58 20.0 +1 300.0 -17.61 -8.45 20.0 +1 300.0 -16.9 -8.18 20.0 +1 300.0 -16.55 -8.04 20.0 +1 300.0 -16.22 -7.89 20.0 +1 300.0 -15.57 -7.59 20.0 +1 300.0 -15.27 -7.44 20.0 +1 300.0 -14.97 -7.28 20.0 +1 300.0 -14.41 -6.95 20.0 +1 300.0 -14.15 -6.77 20.0 +1 300.0 -13.9 -6.6 20.0 +1 300.0 -13.67 -6.42 20.0 +1 300.0 -13.45 -6.24 20.0 +1 300.0 -13.24 -6.05 20.0 +1 300.0 -13.04 -5.86 20.0 +1 300.0 -12.86 -5.67 20.0 +1 300.0 -12.7 -5.47 20.0 +1 300.0 -12.55 -5.27 20.0 +1 300.0 -12.41 -5.07 20.0 +1 300.0 -12.29 -4.86 20.0 +1 300.0 -12.19 -4.65 20.0 +1 300.0 -12.1 -4.44 20.0 +1 300.0 -12.03 -4.22 20.0 +1 300.0 -11.97 -4.0 20.0 +1 300.0 -11.94 -3.78 20.0 +1 300.0 -11.91 -3.56 20.0 +1 300.0 -11.9 -3.33 20.0 +1 300.0 -11.91 -3.1 20.0 +1 300.0 -11.94 -2.86 20.0 +1 300.0 -11.98 -2.63 20.0 +1 300.0 -12.04 -2.39 20.0 +1 300.0 -12.11 -2.14 20.0 +1 300.0 -12.2 -1.9 20.0 +1 300.0 -12.3 -1.67 20.0 +1 300.0 -12.41 -1.44 20.0 +1 300.0 -12.54 -1.2 20.0 +1 300.0 -12.68 -0.96 20.0 +1 300.0 -12.83 -0.73 20.0 +1 300.0 -12.99 -0.49 20.0 +1 300.0 -13.36 0.0 20.0 +1 300.0 -13.55 0.25 20.0 +1 300.0 -13.77 0.49 20.0 +1 300.0 -14.23 0.99 20.0 +1 300.0 -15.27 2.01 20.0 +1 300.0 -15.55 2.27 20.0 +1 300.0 -15.84 2.52 20.0 +1 300.0 -16.45 3.04 20.0 +1 300.0 -17.75 4.09 20.0 +1 300.0 -20.57 6.22 20.0 +1 300.0 -20.93 6.49 20.0 +1 300.0 -21.3 6.75 20.0 +1 300.0 -22.02 7.29 20.0 +1 300.0 -23.46 8.35 20.0 +1 300.0 -23.81 8.62 20.0 +1 300.0 -24.15 8.88 20.0 +1 300.0 -24.84 9.41 20.0 +1 300.0 -26.13 10.46 20.0 +1 300.0 -26.43 10.72 20.0 +1 300.0 -26.73 10.97 20.0 +1 300.0 -27.3 11.49 20.0 +1 300.0 -27.57 11.74 20.0 +1 300.0 -27.83 12.0 20.0 +1 300.0 -28.32 12.5 20.0 +1 300.0 -28.55 12.75 20.0 +1 300.0 -28.77 13.0 20.0 +1 300.0 -28.98 13.24 20.0 +1 300.0 -29.17 13.49 20.0 +1 300.0 -29.36 13.73 20.0 +1 300.0 -29.53 13.97 20.0 +1 300.0 -29.69 14.21 20.0 +1 300.0 -29.83 14.45 20.0 +1 300.0 -29.97 14.7 20.0 +1 300.0 -30.1 14.95 20.0 +1 300.0 -30.21 15.2 20.0 +1 300.0 -30.31 15.45 20.0 +1 300.0 -30.39 15.69 20.0 +1 300.0 -30.45 15.94 20.0 +1 300.0 -30.49 16.18 20.0 +1 300.0 -30.52 16.41 20.0 +1 300.0 -30.53 16.64 20.0 +1 300.0 -30.53 16.87 20.0 +1 300.0 -30.5 17.1 20.0 +1 300.0 -30.47 17.33 20.0 +1 300.0 -30.41 17.55 20.0 +1 300.0 -30.34 17.77 20.0 +1 300.0 -30.25 17.98 20.0 +1 300.0 -30.14 18.19 20.0 +1 300.0 -30.02 18.4 20.0 +1 300.0 -29.88 18.6 20.0 +1 300.0 -29.73 18.8 20.0 +1 300.0 -29.56 19.0 20.0 +1 300.0 -29.38 19.2 20.0 +1 300.0 -29.18 19.39 20.0 +1 300.0 -28.96 19.57 20.0 +1 300.0 -28.73 19.76 20.0 +1 300.0 -28.49 19.93 20.0 +1 300.0 -28.23 20.11 20.0 +1 300.0 -27.96 20.28 20.0 +1 300.0 -27.68 20.45 20.0 +1 300.0 -27.39 20.61 20.0 +1 300.0 -27.08 20.77 20.0 +1 300.0 -26.43 21.08 20.0 +1 300.0 -26.1 21.23 20.0 +1 300.0 -25.75 21.37 20.0 +1 300.0 -25.02 21.65 20.0 +1 300.0 -24.65 21.78 20.0 +1 300.0 -24.27 21.91 20.0 +1 300.0 -23.49 22.15 20.0 +1 300.0 -23.09 22.26 20.0 +1 300.0 -22.69 22.38 20.0 +1 300.0 -21.88 22.58 20.0 +1 300.0 -21.47 22.68 20.0 +1 300.0 -21.05 22.77 20.0 +1 300.0 -20.22 22.95 20.0 +1 300.0 -19.81 23.03 20.0 +1 300.0 -19.4 23.11 20.0 +1 300.0 -18.57 23.25 20.0 +1 300.0 -18.17 23.31 20.0 +1 300.0 -17.77 23.37 20.0 +1 300.0 -16.98 23.48 20.0 +1 300.0 -16.59 23.52 20.0 +1 300.0 -16.21 23.57 20.0 +1 300.0 -15.84 23.61 20.0 +1 300.0 -15.47 23.64 20.0 +1 300.0 -15.12 23.67 20.0 +1 300.0 -14.77 23.7 20.0 +1 300.0 -14.43 23.72 20.0 +1 300.0 -14.1 23.74 20.0 +1 300.0 -13.79 23.75 20.0 +1 300.0 -13.49 23.76 20.0 +1 300.0 -13.2 23.77 20.0 +1 300.0 -12.92 23.77 20.0 +1 300.0 -12.66 23.77 20.0 +1 300.0 -12.4 23.77 20.0 +1 300.0 -12.16 23.76 20.0 +1 300.0 -11.94 23.75 20.0 +1 300.0 -11.72 23.73 20.0 +1 300.0 -11.53 23.71 20.0 +1 300.0 -11.34 23.69 20.0 +1 300.0 -11.17 23.66 20.0 +1 300.0 -11.02 23.63 20.0 +1 300.0 -10.88 23.6 20.0 +1 300.0 -10.75 23.56 20.0 +1 300.0 -10.64 23.52 20.0 +1 300.0 -10.55 23.48 20.0 +1 300.0 -10.47 23.43 20.0 +1 300.0 -10.41 23.39 20.0 +1 300.0 -10.37 23.33 20.0 +1 300.0 -10.34 23.28 20.0 +1 300.0 -10.32 23.22 20.0 +1 300.0 -10.33 23.16 20.0 +1 300.0 -10.34 23.1 20.0 +1 300.0 -10.38 23.03 20.0 +1 300.0 -10.43 22.96 20.0 +1 300.0 -10.5 22.89 20.0 +1 300.0 -10.58 22.82 20.0 +1 300.0 -10.68 22.74 20.0 +1 300.0 -10.79 22.66 20.0 +1 300.0 -10.92 22.58 20.0 +1 300.0 -11.06 22.49 20.0 +1 300.0 -11.22 22.41 20.0 +1 300.0 -11.39 22.32 20.0 +1 300.0 -11.77 22.14 20.0 +1 300.0 -11.98 22.04 20.0 +1 300.0 -12.21 21.95 20.0 +1 300.0 -12.7 21.75 20.0 +1 300.0 -12.96 21.64 20.0 +1 300.0 -13.23 21.54 20.0 +1 300.0 -13.81 21.33 20.0 +1 300.0 -15.07 20.89 20.0 +1 300.0 -15.4 20.78 20.0 +1 300.0 -15.74 20.67 20.0 +1 300.0 -16.44 20.44 20.0 +1 300.0 -17.9 19.98 20.0 +1 300.0 -20.87 19.04 20.0 +1 300.0 -21.21 18.93 20.0 +1 300.0 -21.55 18.82 20.0 +1 300.0 -22.21 18.6 20.0 +1 300.0 -23.47 18.17 20.0 +1 300.0 -23.77 18.07 20.0 +1 300.0 -24.07 17.96 20.0 +1 300.0 -24.63 17.76 20.0 +1 300.0 -25.65 17.36 20.0 +1 300.0 -25.88 17.27 20.0 +1 300.0 -26.1 17.17 20.0 +1 300.0 -26.51 16.98 20.0 +1 300.0 -26.7 16.89 20.0 +1 300.0 -26.87 16.8 20.0 +1 300.0 -27.18 16.63 20.0 +1 300.0 -27.32 16.55 20.0 +1 300.0 -27.45 16.46 20.0 +1 300.0 -27.56 16.39 20.0 +1 300.0 -27.66 16.31 20.0 +1 300.0 -27.74 16.23 20.0 +1 300.0 -27.82 16.16 20.0 +1 300.0 -27.87 16.08 20.0 +1 300.0 -27.92 16.01 20.0 +1 300.0 -27.95 15.95 20.0 +1 300.0 -27.96 15.88 20.0 +1 300.0 -27.96 15.82 20.0 +1 300.0 -27.95 15.76 20.0 +1 300.0 -27.93 15.7 20.0 +1 300.0 -27.89 15.64 20.0 +1 300.0 -27.83 15.59 20.0 +1 300.0 -27.76 15.54 20.0 +1 300.0 -27.68 15.49 20.0 +1 300.0 -27.59 15.44 20.0 +1 300.0 -27.48 15.4 20.0 +1 300.0 -27.35 15.36 20.0 +1 300.0 -27.21 15.32 20.0 +1 300.0 -27.06 15.29 20.0 +1 300.0 -26.9 15.25 20.0 +1 300.0 -26.72 15.22 20.0 +1 300.0 -26.53 15.2 20.0 +1 300.0 -26.33 15.17 20.0 +1 300.0 -26.12 15.15 20.0 +1 300.0 -25.89 15.14 20.0 +1 300.0 -25.65 15.12 20.0 +1 300.0 -25.41 15.11 20.0 +1 300.0 -25.14 15.1 20.0 +1 300.0 -24.88 15.1 20.0 +1 300.0 -24.59 15.1 20.0 +1 300.0 -24.3 15.1 20.0 +1 300.0 -24.0 15.1 20.0 +1 300.0 -23.69 15.11 20.0 +1 300.0 -23.38 15.12 20.0 +1 300.0 -23.05 15.13 20.0 +1 300.0 -22.72 15.15 20.0 +1 300.0 -22.38 15.17 20.0 +1 300.0 -22.0 15.2 20.0 +1 300.0 -21.61 15.23 20.0 +1 300.0 -21.22 15.26 20.0 +1 300.0 -20.82 15.3 20.0 +1 300.0 -20.42 15.34 20.0 +1 300.0 -20.01 15.38 20.0 +1 300.0 -19.18 15.49 20.0 +1 300.0 -18.76 15.55 20.0 +1 300.0 -18.34 15.61 20.0 +1 300.0 -17.49 15.74 20.0 +1 300.0 -17.07 15.82 20.0 +1 300.0 -16.64 15.9 20.0 +1 300.0 -15.8 16.06 20.0 +1 300.0 -15.38 16.15 20.0 +1 300.0 -14.97 16.25 20.0 +1 300.0 -14.15 16.45 20.0 +1 300.0 -13.75 16.55 20.0 +1 300.0 -13.35 16.66 20.0 +1 300.0 -12.58 16.89 20.0 +1 300.0 -12.21 17.02 20.0 +1 300.0 -11.84 17.14 20.0 +1 300.0 -11.14 17.41 20.0 +1 300.0 -10.8 17.54 20.0 +1 300.0 -10.48 17.68 20.0 +1 300.0 -9.86 17.98 20.0 +1 300.0 -9.57 18.13 20.0 +1 300.0 -9.29 18.28 20.0 +1 300.0 -9.02 18.44 20.0 +1 300.0 -8.77 18.6 20.0 +1 300.0 -8.53 18.77 20.0 +1 300.0 -8.3 18.94 20.0 +1 300.0 -8.09 19.11 20.0 +1 300.0 -7.9 19.29 20.0 +1 300.0 -7.72 19.47 20.0 +1 300.0 -7.55 19.65 20.0 +1 300.0 -7.4 19.84 20.0 +1 300.0 -7.27 20.03 20.0 +1 300.0 -7.15 20.22 20.0 +1 300.0 -7.05 20.42 20.0 +1 300.0 -6.96 20.62 20.0 +1 300.0 -6.89 20.82 20.0 +1 300.0 -6.84 21.02 20.0 +1 300.0 -6.81 21.23 20.0 +1 300.0 -6.79 21.44 20.0 +1 300.0 -6.78 21.65 20.0 +1 300.0 -6.8 21.87 20.0 +1 300.0 -6.83 22.09 20.0 +1 300.0 -6.87 22.31 20.0 +1 300.0 -6.93 22.53 20.0 +1 300.0 -7.01 22.76 20.0 +1 300.0 -7.1 22.99 20.0 +1 300.0 -7.21 23.22 20.0 +1 300.0 -7.34 23.45 20.0 +1 300.0 -7.48 23.68 20.0 +1 300.0 -7.63 23.92 20.0 +1 300.0 -7.98 24.4 20.0 +1 300.0 -8.16 24.63 20.0 +1 300.0 -8.36 24.86 20.0 +1 300.0 -8.78 25.32 20.0 +1 300.0 -9.75 26.25 20.0 +1 300.0 -10.02 26.49 20.0 +1 300.0 -10.29 26.73 20.0 +1 300.0 -10.86 27.2 20.0 +1 300.0 -12.09 28.17 20.0 +1 300.0 -12.41 28.41 20.0 +1 300.0 -12.73 28.66 20.0 +1 300.0 -13.39 29.14 20.0 +1 300.0 -14.75 30.12 20.0 +1 300.0 -17.46 32.08 20.0 +1 300.0 -17.79 32.32 20.0 +1 300.0 -18.11 32.56 20.0 +1 300.0 -18.74 33.04 20.0 +1 300.0 -19.93 33.99 20.0 +1 300.0 -20.21 34.23 20.0 +1 300.0 -20.48 34.46 20.0 +1 300.0 -21.0 34.93 20.0 +1 300.0 -21.24 35.16 20.0 +1 300.0 -21.47 35.39 20.0 +1 300.0 -21.91 35.84 20.0 +1 300.0 -22.11 36.06 20.0 +1 300.0 -22.3 36.28 20.0 +1 300.0 -22.48 36.51 20.0 +1 300.0 -22.65 36.72 20.0 +1 300.0 -22.8 36.94 20.0 +1 300.0 -22.94 37.16 20.0 +1 300.0 -23.07 37.37 20.0 +1 300.0 -23.18 37.58 20.0 +1 300.0 -23.29 37.79 20.0 +1 300.0 -23.37 37.99 20.0 +1 300.0 -23.45 38.2 20.0 +1 300.0 -23.51 38.4 20.0 +1 300.0 -23.55 38.6 20.0 +1 300.0 -23.59 38.8 20.0 +1 300.0 -23.6 38.99 20.0 +1 300.0 -23.61 39.18 20.0 +1 300.0 -23.59 39.39 20.0 +1 300.0 -23.56 39.59 20.0 +1 300.0 -23.52 39.79 20.0 +1 300.0 -23.45 39.98 20.0 +1 300.0 -23.37 40.17 20.0 +1 300.0 -23.27 40.36 20.0 +1 300.0 -23.16 40.55 20.0 +1 300.0 -23.03 40.73 20.0 +1 300.0 -22.88 40.91 20.0 +1 300.0 -22.72 41.09 20.0 +1 300.0 -22.54 41.26 20.0 +1 300.0 -22.34 41.42 20.0 +1 300.0 -22.13 41.59 20.0 +1 300.0 -21.91 41.75 20.0 +1 300.0 -21.67 41.91 20.0 +1 300.0 -21.41 42.06 20.0 +1 300.0 -21.14 42.21 20.0 +1 300.0 -20.86 42.35 20.0 +1 300.0 -20.57 42.49 20.0 +1 300.0 -20.26 42.63 20.0 +1 300.0 -19.94 42.76 20.0 +1 300.0 -19.6 42.89 20.0 +1 300.0 -18.91 43.14 20.0 +1 300.0 -18.54 43.25 20.0 +1 300.0 -18.17 43.37 20.0 +1 300.0 -17.79 43.47 20.0 +1 300.0 -17.39 43.58 20.0 +1 300.0 -17.0 43.68 20.0 +1 300.0 -16.59 43.77 20.0 +1 300.0 -15.76 43.95 20.0 +1 300.0 -15.34 44.03 20.0 +1 300.0 -14.91 44.11 20.0 +1 300.0 -14.48 44.18 20.0 +1 300.0 -14.04 44.25 20.0 +1 300.0 -13.61 44.32 20.0 +1 300.0 -13.17 44.38 20.0 +1 300.0 -12.29 44.49 20.0 +1 300.0 -11.85 44.53 20.0 +1 300.0 -11.41 44.58 20.0 +1 300.0 -10.98 44.62 20.0 +1 300.0 -10.55 44.65 20.0 +1 300.0 -10.12 44.68 20.0 +1 300.0 -9.69 44.7 20.0 +1 300.0 -9.27 44.73 20.0 +1 300.0 -8.86 44.74 20.0 +1 300.0 -8.45 44.76 20.0 +1 300.0 -8.04 44.76 20.0 +1 300.0 -7.65 44.77 20.0 +1 300.0 -7.26 44.77 20.0 +1 300.0 -6.88 44.76 20.0 +1 300.0 -6.52 44.75 20.0 +1 300.0 -6.16 44.74 20.0 +1 300.0 -5.81 44.72 20.0 +1 300.0 -5.47 44.7 20.0 +1 300.0 -5.14 44.68 20.0 +1 300.0 -4.83 44.65 20.0 +1 300.0 -4.53 44.61 20.0 +1 300.0 -4.24 44.58 20.0 +1 300.0 -3.96 44.53 20.0 +1 300.0 -3.7 44.49 20.0 +1 300.0 -3.46 44.44 20.0 +1 300.0 -3.23 44.39 20.0 +1 300.0 -3.01 44.33 20.0 +1 300.0 -2.81 44.27 20.0 +1 300.0 -2.63 44.21 20.0 +1 300.0 -2.46 44.14 20.0 +1 300.0 -2.31 44.07 20.0 +1 300.0 -2.17 44.0 20.0 +1 300.0 -2.04 43.92 20.0 +1 300.0 -1.94 43.84 20.0 +1 300.0 -1.85 43.76 20.0 +1 300.0 -1.77 43.67 20.0 +1 300.0 -1.72 43.59 20.0 +1 300.0 -1.67 43.49 20.0 +1 300.0 -1.65 43.4 20.0 +1 300.0 -1.64 43.3 20.0 +1 300.0 -1.65 43.2 20.0 +1 300.0 -1.67 43.09 20.0 +1 300.0 -1.71 42.99 20.0 +1 300.0 -1.76 42.88 20.0 +1 300.0 -1.83 42.77 20.0 +1 300.0 -1.92 42.65 20.0 +1 300.0 -2.02 42.53 20.0 +1 300.0 -2.13 42.41 20.0 +1 300.0 -2.27 42.29 20.0 +1 300.0 -2.41 42.17 20.0 +1 300.0 -2.57 42.04 20.0 +1 300.0 -2.93 41.78 20.0 +1 300.0 -3.13 41.65 20.0 +1 300.0 -3.35 41.51 20.0 +1 300.0 -3.81 41.23 20.0 +1 300.0 -4.06 41.09 20.0 +1 300.0 -4.32 40.95 20.0 +1 300.0 -4.87 40.66 20.0 +1 300.0 -6.08 40.07 20.0 +1 300.0 -8.79 38.84 20.0 +1 300.0 -9.14 38.68 20.0 +1 300.0 -9.49 38.53 20.0 +1 300.0 -10.21 38.21 20.0 +1 300.0 -11.61 37.58 20.0 +1 300.0 -14.21 36.34 20.0 +1 300.0 -14.49 36.2 20.0 +1 300.0 -14.76 36.06 20.0 +1 300.0 -15.26 35.78 20.0 +1 300.0 -15.5 35.65 20.0 +1 300.0 -15.73 35.51 20.0 +1 300.0 -16.16 35.24 20.0 +1 300.0 -16.36 35.11 20.0 +1 300.0 -16.55 34.98 20.0 +1 300.0 -16.73 34.84 20.0 +1 300.0 -16.89 34.72 20.0 +1 300.0 -17.05 34.59 20.0 +1 300.0 -17.19 34.47 20.0 +1 300.0 -17.32 34.34 20.0 +1 300.0 -17.43 34.22 20.0 +1 300.0 -17.54 34.1 20.0 +1 300.0 -17.63 33.98 20.0 +1 300.0 -17.7 33.87 20.0 +1 300.0 -17.76 33.75 20.0 +1 300.0 -17.81 33.64 20.0 +1 300.0 -17.85 33.53 20.0 +1 300.0 -17.87 33.42 20.0 +1 300.0 -17.88 33.31 20.0 +1 300.0 -17.87 33.21 20.0 +1 300.0 -17.84 33.11 20.0 +1 300.0 -17.81 33.01 20.0 +1 300.0 -17.76 32.91 20.0 +1 300.0 -17.7 32.82 20.0 +1 300.0 -17.62 32.73 20.0 +1 300.0 -17.52 32.64 20.0 +1 300.0 -17.42 32.55 20.0 +1 300.0 -17.3 32.47 20.0 +1 300.0 -17.16 32.39 20.0 +1 300.0 -17.01 32.31 20.0 +1 300.0 -16.85 32.23 20.0 +1 300.0 -16.68 32.16 20.0 +1 300.0 -16.49 32.09 20.0 +1 300.0 -16.29 32.02 20.0 +1 300.0 -16.07 31.96 20.0 +1 300.0 -15.85 31.89 20.0 +1 300.0 -15.61 31.84 20.0 +1 300.0 -15.36 31.78 20.0 +1 300.0 -15.1 31.73 20.0 +1 300.0 -14.83 31.68 20.0 +1 300.0 -14.54 31.63 20.0 +1 300.0 -14.25 31.59 20.0 +1 300.0 -13.95 31.55 20.0 +1 300.0 -13.63 31.51 20.0 +1 300.0 -13.31 31.48 20.0 +1 300.0 -12.98 31.44 20.0 +1 300.0 -12.64 31.42 20.0 +1 300.0 -12.29 31.39 20.0 +1 300.0 -11.93 31.37 20.0 +1 300.0 -11.57 31.35 20.0 +1 300.0 -11.2 31.34 20.0 +1 300.0 -10.82 31.33 20.0 +1 300.0 -10.44 31.32 20.0 +1 300.0 -10.06 31.31 20.0 +1 300.0 -9.67 31.31 20.0 +1 300.0 -9.27 31.32 20.0 +1 300.0 -8.87 31.32 20.0 +1 300.0 -8.47 31.33 20.0 +1 300.0 -8.07 31.34 20.0 +1 300.0 -7.63 31.36 20.0 +1 300.0 -7.19 31.38 20.0 +1 300.0 -6.75 31.4 20.0 +1 300.0 -6.3 31.43 20.0 +1 300.0 -5.86 31.47 20.0 +1 300.0 -5.42 31.5 20.0 +1 300.0 -4.98 31.54 20.0 +1 300.0 -4.55 31.59 20.0 +1 300.0 -4.11 31.64 20.0 +1 300.0 -3.68 31.69 20.0 +1 300.0 -3.26 31.75 20.0 +1 300.0 -2.84 31.81 20.0 +1 300.0 -2.43 31.87 20.0 +1 300.0 -2.02 31.94 20.0 +1 300.0 -1.23 32.09 20.0 +1 300.0 -0.85 32.17 20.0 +1 300.0 -0.47 32.26 20.0 +1 300.0 -0.11 32.34 20.0 +1 300.0 0.24 32.43 20.0 +1 300.0 0.59 32.53 20.0 +1 300.0 0.92 32.63 20.0 +1 300.0 1.55 32.84 20.0 +1 300.0 1.84 32.95 20.0 +1 300.0 2.12 33.06 20.0 +1 300.0 2.39 33.18 20.0 +1 300.0 2.65 33.3 20.0 +1 300.0 2.89 33.42 20.0 +1 300.0 3.11 33.55 20.0 +1 300.0 3.32 33.68 20.0 +1 300.0 3.52 33.81 20.0 +1 300.0 3.7 33.95 20.0 +1 300.0 3.86 34.09 20.0 +1 300.0 4.01 34.24 20.0 +1 300.0 4.14 34.38 20.0 +1 300.0 4.26 34.53 20.0 +1 300.0 4.36 34.69 20.0 +1 300.0 4.44 34.84 20.0 +1 300.0 4.5 35.0 20.0 +1 300.0 4.55 35.16 20.0 +1 300.0 4.59 35.33 20.0 +1 300.0 4.6 35.49 20.0 +1 300.0 4.6 35.66 20.0 +1 300.0 4.58 35.83 20.0 +1 300.0 4.55 36.01 20.0 +1 300.0 4.5 36.19 20.0 +1 300.0 4.43 36.36 20.0 +1 300.0 4.35 36.55 20.0 +1 300.0 4.25 36.73 20.0 +1 300.0 4.14 36.92 20.0 +1 300.0 4.01 37.1 20.0 +1 300.0 3.87 37.29 20.0 +1 300.0 3.71 37.49 20.0 +1 300.0 3.36 37.88 20.0 +1 300.0 3.16 38.07 20.0 +1 300.0 2.94 38.27 20.0 +1 300.0 2.48 38.67 20.0 +1 300.0 1.43 39.5 20.0 +1 300.0 1.16 39.69 20.0 +1 300.0 0.88 39.88 20.0 +1 300.0 0.3 40.28 20.0 +1 300.0 -0.93 41.07 20.0 +1 300.0 -3.54 42.67 20.0 +1 300.0 -3.87 42.87 20.0 +1 300.0 -4.2 43.07 20.0 +1 300.0 -4.86 43.46 20.0 +1 300.0 -6.14 44.25 20.0 +1 300.0 -6.44 44.45 20.0 +1 300.0 -6.75 44.64 20.0 +1 300.0 -7.34 45.03 20.0 +1 300.0 -8.43 45.79 20.0 +1 300.0 -8.68 45.98 20.0 +1 300.0 -8.93 46.16 20.0 +1 300.0 -9.38 46.53 20.0 +1 300.0 -9.6 46.71 20.0 +1 300.0 -9.8 46.89 20.0 +1 300.0 -10.17 47.24 20.0 +1 300.0 -10.34 47.41 20.0 +1 300.0 -10.49 47.59 20.0 +1 300.0 -10.64 47.76 20.0 +1 300.0 -10.77 47.92 20.0 +1 300.0 -10.88 48.09 20.0 +1 300.0 -10.99 48.26 20.0 +1 300.0 -11.08 48.42 20.0 +1 300.0 -11.15 48.58 20.0 +1 300.0 -11.22 48.73 20.0 +1 300.0 -11.27 48.89 20.0 +1 300.0 -11.3 49.04 20.0 +1 300.0 -11.32 49.19 20.0 +1 300.0 -11.33 49.34 20.0 +1 300.0 -11.32 49.48 20.0 +1 300.0 -11.3 49.62 20.0 +1 300.0 -11.26 49.76 20.0 +1 300.0 -11.21 49.9 20.0 +1 300.0 -11.14 50.03 20.0 +1 300.0 -11.06 50.16 20.0 +1 300.0 -10.96 50.29 20.0 +1 300.0 -10.85 50.42 20.0 +1 300.0 -10.73 50.54 20.0 +1 300.0 -10.59 50.66 20.0 +1 300.0 -10.44 50.77 20.0 +1 300.0 -10.28 50.89 20.0 +1 300.0 -10.1 50.99 20.0 +1 300.0 -9.91 51.1 20.0 +1 300.0 -9.71 51.2 20.0 +1 300.0 -9.5 51.3 20.0 +1 300.0 -9.27 51.39 20.0 +1 300.0 -8.78 51.57 20.0 +1 300.0 -8.52 51.66 20.0 +1 300.0 -8.25 51.74 20.0 +1 300.0 -7.97 51.82 20.0 +1 300.0 -7.67 51.9 20.0 +1 300.0 -7.37 51.97 20.0 +1 300.0 -7.06 52.04 20.0 +1 300.0 -6.41 52.16 20.0 +1 300.0 -6.07 52.22 20.0 +1 300.0 -5.72 52.28 20.0 +1 300.0 -5.37 52.33 20.0 +1 300.0 -5.01 52.38 20.0 +1 300.0 -4.64 52.42 20.0 +1 300.0 -4.26 52.46 20.0 +1 300.0 -3.89 52.5 20.0 +1 300.0 -3.5 52.53 20.0 +1 300.0 -3.11 52.56 20.0 +1 300.0 -2.72 52.59 20.0 +1 300.0 -2.32 52.61 20.0 +1 300.0 -1.92 52.63 20.0 +1 300.0 -1.52 52.65 20.0 +1 300.0 -1.12 52.66 20.0 +1 300.0 -0.71 52.67 20.0 +1 300.0 -0.3 52.67 20.0 +1 300.0 0.1 52.67 20.0 +1 300.0 0.51 52.67 20.0 +1 300.0 0.92 52.66 20.0 +1 300.0 1.32 52.65 20.0 +1 300.0 1.72 52.64 20.0 +1 300.0 2.13 52.62 20.0 +1 300.0 2.52 52.6 20.0 +1 300.0 2.92 52.58 20.0 +1 300.0 3.31 52.55 20.0 +1 300.0 3.7 52.52 20.0 +1 300.0 4.08 52.48 20.0 +1 300.0 4.46 52.44 20.0 +1 300.0 4.83 52.4 20.0 +1 300.0 5.19 52.35 20.0 +1 300.0 5.55 52.3 20.0 +1 300.0 5.9 52.25 20.0 +1 300.0 6.24 52.19 20.0 +1 300.0 6.58 52.13 20.0 +1 300.0 6.9 52.07 20.0 +1 300.0 7.22 52.0 20.0 +1 300.0 7.53 51.93 20.0 +1 300.0 7.82 51.86 20.0 +1 300.0 8.39 51.7 20.0 +1 300.0 8.66 51.62 20.0 +1 300.0 8.91 51.53 20.0 +1 300.0 9.15 51.44 20.0 +1 300.0 9.39 51.35 20.0 +1 300.0 9.61 51.25 20.0 +1 300.0 9.81 51.15 20.0 +1 300.0 10.01 51.05 20.0 +1 300.0 10.19 50.94 20.0 +1 300.0 10.37 50.82 20.0 +1 300.0 10.54 50.7 20.0 +1 300.0 10.69 50.57 20.0 +1 300.0 10.83 50.44 20.0 +1 300.0 10.95 50.31 20.0 +1 300.0 11.05 50.17 20.0 +1 300.0 11.14 50.04 20.0 +1 300.0 11.21 49.89 20.0 +1 300.0 11.26 49.75 20.0 +1 300.0 11.3 49.6 20.0 +1 300.0 11.32 49.45 20.0 +1 300.0 11.33 49.29 20.0 +1 300.0 11.32 49.14 20.0 +1 300.0 11.29 48.98 20.0 +1 300.0 11.24 48.81 20.0 +1 300.0 11.19 48.65 20.0 +1 300.0 11.11 48.48 20.0 +1 300.0 11.02 48.31 20.0 +1 300.0 10.91 48.13 20.0 +1 300.0 10.79 47.96 20.0 +1 300.0 10.65 47.78 20.0 +1 300.0 10.5 47.6 20.0 +1 300.0 10.16 47.23 20.0 +1 300.0 9.97 47.04 20.0 +1 300.0 9.76 46.85 20.0 +1 300.0 9.31 46.47 20.0 +1 300.0 8.28 45.68 20.0 +1 300.0 8.0 45.48 20.0 +1 300.0 7.71 45.28 20.0 +1 300.0 7.1 44.87 20.0 +1 300.0 5.8 44.04 20.0 +1 300.0 3.01 42.35 20.0 +1 300.0 2.66 42.14 20.0 +1 300.0 2.31 41.92 20.0 +1 300.0 1.62 41.5 20.0 +1 300.0 0.27 40.65 20.0 +1 300.0 -0.05 40.44 20.0 +1 300.0 -0.36 40.23 20.0 +1 300.0 -0.97 39.82 20.0 +1 300.0 -2.09 38.99 20.0 +1 300.0 -2.32 38.8 20.0 +1 300.0 -2.55 38.62 20.0 +1 300.0 -2.97 38.24 20.0 +1 300.0 -3.17 38.06 20.0 +1 300.0 -3.35 37.88 20.0 +1 300.0 -3.69 37.52 20.0 +1 300.0 -3.84 37.34 20.0 +1 300.0 -3.97 37.16 20.0 +1 300.0 -4.1 36.99 20.0 +1 300.0 -4.21 36.81 20.0 +1 300.0 -4.3 36.64 20.0 +1 300.0 -4.39 36.47 20.0 +1 300.0 -4.46 36.3 20.0 +1 300.0 -4.51 36.14 20.0 +1 300.0 -4.56 35.98 20.0 +1 300.0 -4.59 35.81 20.0 +1 300.0 -4.6 35.66 20.0 +1 300.0 -4.6 35.5 20.0 +1 300.0 -4.59 35.34 20.0 +1 300.0 -4.56 35.19 20.0 +1 300.0 -4.52 35.04 20.0 +1 300.0 -4.46 34.89 20.0 +1 300.0 -4.39 34.75 20.0 +1 300.0 -4.31 34.6 20.0 +1 300.0 -4.21 34.47 20.0 +1 300.0 -4.09 34.33 20.0 +1 300.0 -3.97 34.19 20.0 +1 300.0 -3.83 34.06 20.0 +1 300.0 -3.67 33.93 20.0 +1 300.0 -3.5 33.8 20.0 +1 300.0 -3.32 33.68 20.0 +1 300.0 -3.13 33.56 20.0 +1 300.0 -2.92 33.44 20.0 +1 300.0 -2.7 33.32 20.0 +1 300.0 -2.47 33.21 20.0 +1 300.0 -2.22 33.1 20.0 +1 300.0 -1.7 32.89 20.0 +1 300.0 -1.42 32.79 20.0 +1 300.0 -1.13 32.7 20.0 +1 300.0 -0.83 32.6 20.0 +1 300.0 -0.52 32.51 20.0 +1 300.0 -0.2 32.42 20.0 +1 300.0 0.13 32.34 20.0 +1 300.0 0.81 32.18 20.0 +1 300.0 1.17 32.1 20.0 +1 300.0 1.53 32.03 20.0 +1 300.0 1.9 31.96 20.0 +1 300.0 2.27 31.9 20.0 +1 300.0 2.65 31.84 20.0 +1 300.0 3.04 31.78 20.0 +1 300.0 3.82 31.67 20.0 +1 300.0 4.22 31.63 20.0 +1 300.0 4.62 31.58 20.0 +1 300.0 5.03 31.54 20.0 +1 300.0 5.43 31.5 20.0 +1 300.0 5.84 31.47 20.0 +1 300.0 6.25 31.44 20.0 +1 300.0 6.66 31.41 20.0 +1 300.0 7.07 31.39 20.0 +1 300.0 7.51 31.36 20.0 +1 300.0 7.96 31.34 20.0 +1 300.0 8.4 31.33 20.0 +1 300.0 8.84 31.32 20.0 +1 300.0 9.27 31.32 20.0 +1 300.0 9.7 31.31 20.0 +1 300.0 10.12 31.32 20.0 +1 300.0 10.54 31.32 20.0 +1 300.0 10.96 31.33 20.0 +1 300.0 11.36 31.34 20.0 +1 300.0 11.76 31.36 20.0 +1 300.0 12.15 31.38 20.0 +1 300.0 12.53 31.41 20.0 +1 300.0 12.91 31.44 20.0 +1 300.0 13.27 31.47 20.0 +1 300.0 13.62 31.51 20.0 +1 300.0 13.96 31.55 20.0 +1 300.0 14.29 31.59 20.0 +1 300.0 14.61 31.64 20.0 +1 300.0 14.92 31.69 20.0 +1 300.0 15.21 31.75 20.0 +1 300.0 15.49 31.81 20.0 +1 300.0 15.75 31.87 20.0 +1 300.0 16.0 31.94 20.0 +1 300.0 16.24 32.01 20.0 +1 300.0 16.46 32.08 20.0 +1 300.0 16.67 32.16 20.0 +1 300.0 16.86 32.23 20.0 +1 300.0 17.03 32.32 20.0 +1 300.0 17.19 32.4 20.0 +1 300.0 17.33 32.49 20.0 +1 300.0 17.46 32.59 20.0 +1 300.0 17.57 32.68 20.0 +1 300.0 17.66 32.78 20.0 +1 300.0 17.74 32.88 20.0 +1 300.0 17.8 32.99 20.0 +1 300.0 17.84 33.09 20.0 +1 300.0 17.87 33.2 20.0 +1 300.0 17.88 33.32 20.0 +1 300.0 17.87 33.43 20.0 +1 300.0 17.84 33.55 20.0 +1 300.0 17.8 33.67 20.0 +1 300.0 17.74 33.79 20.0 +1 300.0 17.67 33.92 20.0 +1 300.0 17.58 34.05 20.0 +1 300.0 17.47 34.17 20.0 +1 300.0 17.35 34.31 20.0 +1 300.0 17.21 34.44 20.0 +1 300.0 17.06 34.58 20.0 +1 300.0 16.9 34.72 20.0 +1 300.0 16.52 35.0 20.0 +1 300.0 16.31 35.14 20.0 +1 300.0 16.09 35.28 20.0 +1 300.0 15.61 35.58 20.0 +1 300.0 15.36 35.73 20.0 +1 300.0 15.09 35.88 20.0 +1 300.0 14.52 36.18 20.0 +1 300.0 13.28 36.81 20.0 +1 300.0 12.96 36.96 20.0 +1 300.0 12.63 37.12 20.0 +1 300.0 11.95 37.43 20.0 +1 300.0 10.56 38.05 20.0 +1 300.0 7.75 39.3 20.0 +1 300.0 7.4 39.46 20.0 +1 300.0 7.07 39.61 20.0 +1 300.0 6.41 39.91 20.0 +1 300.0 5.17 40.51 20.0 +1 300.0 4.88 40.66 20.0 +1 300.0 4.6 40.8 20.0 +1 300.0 4.07 41.09 20.0 +1 300.0 3.83 41.23 20.0 +1 300.0 3.59 41.37 20.0 +1 300.0 3.15 41.64 20.0 +1 300.0 2.95 41.77 20.0 +1 300.0 2.76 41.9 20.0 +1 300.0 2.58 42.03 20.0 +1 300.0 2.42 42.16 20.0 +1 300.0 2.28 42.28 20.0 +1 300.0 2.15 42.4 20.0 +1 300.0 2.03 42.52 20.0 +1 300.0 1.93 42.64 20.0 +1 300.0 1.84 42.75 20.0 +1 300.0 1.77 42.87 20.0 +1 300.0 1.71 42.98 20.0 +1 300.0 1.67 43.08 20.0 +1 300.0 1.65 43.19 20.0 +1 300.0 1.64 43.29 20.0 +1 300.0 1.65 43.39 20.0 +1 300.0 1.67 43.48 20.0 +1 300.0 1.71 43.58 20.0 +1 300.0 1.76 43.66 20.0 +1 300.0 1.84 43.75 20.0 +1 300.0 1.93 43.84 20.0 +1 300.0 2.03 43.91 20.0 +1 300.0 2.15 43.99 20.0 +1 300.0 2.29 44.06 20.0 +1 300.0 2.44 44.13 20.0 +1 300.0 2.6 44.2 20.0 +1 300.0 2.79 44.27 20.0 +1 300.0 2.98 44.32 20.0 +1 300.0 3.19 44.38 20.0 +1 300.0 3.42 44.43 20.0 +1 300.0 3.66 44.48 20.0 +1 300.0 3.92 44.53 20.0 +1 300.0 4.18 44.57 20.0 +1 300.0 4.44 44.6 20.0 +1 300.0 4.71 44.63 20.0 +1 300.0 5.0 44.66 20.0 +1 300.0 5.29 44.69 20.0 +1 300.0 5.59 44.71 20.0 +1 300.0 5.9 44.73 20.0 +1 300.0 6.22 44.74 20.0 +1 300.0 6.55 44.75 20.0 +1 300.0 6.89 44.76 20.0 +1 300.0 7.24 44.77 20.0 +1 300.0 7.59 44.77 20.0 +1 300.0 7.95 44.76 20.0 +1 300.0 8.31 44.76 20.0 +1 300.0 8.68 44.75 20.0 +1 300.0 9.06 44.73 20.0 +1 300.0 9.44 44.72 20.0 +1 300.0 9.83 44.7 20.0 +1 300.0 10.22 44.67 20.0 +1 300.0 10.61 44.64 20.0 +1 300.0 11.0 44.61 20.0 +1 300.0 11.4 44.58 20.0 +1 300.0 11.8 44.54 20.0 +1 300.0 12.6 44.45 20.0 +1 300.0 13.0 44.4 20.0 +1 300.0 13.4 44.35 20.0 +1 300.0 13.8 44.29 20.0 +1 300.0 14.2 44.23 20.0 +1 300.0 14.6 44.16 20.0 +1 300.0 14.99 44.1 20.0 +1 300.0 15.77 43.95 20.0 +1 300.0 16.15 43.87 20.0 +1 300.0 16.52 43.79 20.0 +1 300.0 17.26 43.61 20.0 +1 300.0 17.62 43.52 20.0 +1 300.0 17.98 43.42 20.0 +1 300.0 18.66 43.22 20.0 +1 300.0 18.99 43.11 20.0 +1 300.0 19.31 43.0 20.0 +1 300.0 19.63 42.88 20.0 +1 300.0 19.93 42.77 20.0 +1 300.0 20.22 42.65 20.0 +1 300.0 20.51 42.52 20.0 +1 300.0 21.04 42.26 20.0 +1 300.0 21.29 42.13 20.0 +1 300.0 21.53 41.99 20.0 +1 300.0 21.76 41.85 20.0 +1 300.0 21.97 41.7 20.0 +1 300.0 22.18 41.56 20.0 +1 300.0 22.37 41.41 20.0 +1 300.0 22.54 41.25 20.0 +1 300.0 22.71 41.1 20.0 +1 300.0 22.86 40.94 20.0 +1 300.0 23.0 40.77 20.0 +1 300.0 23.12 40.61 20.0 +1 300.0 23.23 40.44 20.0 +1 300.0 23.32 40.27 20.0 +1 300.0 23.41 40.09 20.0 +1 300.0 23.48 39.92 20.0 +1 300.0 23.53 39.74 20.0 +1 300.0 23.57 39.54 20.0 +1 300.0 23.6 39.34 20.0 +1 300.0 23.61 39.14 20.0 +1 300.0 23.6 38.93 20.0 +1 300.0 23.57 38.72 20.0 +1 300.0 23.53 38.51 20.0 +1 300.0 23.48 38.29 20.0 +1 300.0 23.4 38.08 20.0 +1 300.0 23.32 37.85 20.0 +1 300.0 23.21 37.63 20.0 +1 300.0 23.09 37.41 20.0 +1 300.0 22.95 37.18 20.0 +1 300.0 22.8 36.95 20.0 +1 300.0 22.64 36.72 20.0 +1 300.0 22.27 36.24 20.0 +1 300.0 22.06 36.01 20.0 +1 300.0 21.84 35.77 20.0 +1 300.0 21.36 35.28 20.0 +1 300.0 20.27 34.28 20.0 +1 300.0 19.98 34.03 20.0 +1 300.0 19.67 33.78 20.0 +1 300.0 19.03 33.27 20.0 +1 300.0 17.67 32.23 20.0 +1 300.0 17.32 31.97 20.0 +1 300.0 16.96 31.71 20.0 +1 300.0 16.24 31.19 20.0 +1 300.0 14.77 30.14 20.0 +1 300.0 14.4 29.88 20.0 +1 300.0 14.04 29.61 20.0 +1 300.0 13.32 29.09 20.0 +1 300.0 11.92 28.04 20.0 +1 300.0 11.59 27.78 20.0 +1 300.0 11.26 27.52 20.0 +1 300.0 10.63 27.01 20.0 +1 300.0 10.32 26.75 20.0 +1 300.0 10.03 26.5 20.0 +1 300.0 9.47 25.99 20.0 +1 300.0 9.2 25.74 20.0 +1 300.0 8.95 25.49 20.0 +1 300.0 8.48 24.99 20.0 +1 300.0 8.27 24.75 20.0 +1 300.0 8.06 24.5 20.0 +1 300.0 7.7 24.02 20.0 +1 300.0 7.55 23.8 20.0 +1 300.0 7.41 23.58 20.0 +1 300.0 7.29 23.36 20.0 +1 300.0 7.18 23.14 20.0 +1 300.0 7.08 22.93 20.0 +1 300.0 7.0 22.71 20.0 +1 300.0 6.92 22.5 20.0 +1 300.0 6.87 22.3 20.0 +1 300.0 6.83 22.09 20.0 +1 300.0 6.8 21.88 20.0 +1 300.0 6.78 21.68 20.0 +1 300.0 6.79 21.48 20.0 +1 300.0 6.8 21.29 20.0 +1 300.0 6.83 21.09 20.0 +1 300.0 6.87 20.9 20.0 +1 300.0 6.93 20.71 20.0 +1 300.0 7.0 20.52 20.0 +1 300.0 7.09 20.34 20.0 +1 300.0 7.19 20.15 20.0 +1 300.0 7.31 19.98 20.0 +1 300.0 7.43 19.8 20.0 +1 300.0 7.58 19.63 20.0 +1 300.0 7.73 19.45 20.0 +1 300.0 7.9 19.29 20.0 +1 300.0 8.09 19.12 20.0 +1 300.0 8.28 18.96 20.0 +1 300.0 8.49 18.8 20.0 +1 300.0 8.71 18.64 20.0 +1 300.0 8.94 18.49 20.0 +1 300.0 9.19 18.34 20.0 +1 300.0 9.71 18.05 20.0 +1 300.0 9.99 17.91 20.0 +1 300.0 10.28 17.77 20.0 +1 300.0 10.88 17.51 20.0 +1 300.0 11.2 17.38 20.0 +1 300.0 11.53 17.26 20.0 +1 300.0 12.2 17.02 20.0 +1 300.0 12.55 16.91 20.0 +1 300.0 12.91 16.8 20.0 +1 300.0 13.64 16.58 20.0 +1 300.0 14.01 16.48 20.0 +1 300.0 14.39 16.39 20.0 +1 300.0 15.16 16.2 20.0 +1 300.0 15.55 16.12 20.0 +1 300.0 15.94 16.04 20.0 +1 300.0 16.73 15.88 20.0 +1 300.0 17.13 15.81 20.0 +1 300.0 17.52 15.74 20.0 +1 300.0 18.32 15.61 20.0 +1 300.0 18.71 15.55 20.0 +1 300.0 19.1 15.5 20.0 +1 300.0 19.49 15.45 20.0 +1 300.0 19.88 15.4 20.0 +1 300.0 20.26 15.36 20.0 +1 300.0 20.64 15.32 20.0 +1 300.0 21.02 15.28 20.0 +1 300.0 21.39 15.24 20.0 +1 300.0 21.74 15.21 20.0 +1 300.0 22.09 15.19 20.0 +1 300.0 22.44 15.17 20.0 +1 300.0 22.78 15.15 20.0 +1 300.0 23.11 15.13 20.0 +1 300.0 23.43 15.12 20.0 +1 300.0 23.75 15.11 20.0 +1 300.0 24.05 15.1 20.0 +1 300.0 24.35 15.1 20.0 +1 300.0 24.64 15.1 20.0 +1 300.0 24.91 15.1 20.0 +1 300.0 25.18 15.1 20.0 +1 300.0 25.44 15.11 20.0 +1 300.0 25.68 15.12 20.0 +1 300.0 25.92 15.14 20.0 +1 300.0 26.14 15.16 20.0 +1 300.0 26.35 15.18 20.0 +1 300.0 26.55 15.2 20.0 +1 300.0 26.74 15.23 20.0 +1 300.0 26.91 15.26 20.0 +1 300.0 27.08 15.29 20.0 +1 300.0 27.23 15.32 20.0 +1 300.0 27.36 15.36 20.0 +1 300.0 27.48 15.4 20.0 +1 300.0 27.59 15.45 20.0 +1 300.0 27.69 15.49 20.0 +1 300.0 27.77 15.54 20.0 +1 300.0 27.84 15.59 20.0 +1 300.0 27.89 15.64 20.0 +1 300.0 27.93 15.7 20.0 +1 300.0 27.95 15.76 20.0 +1 300.0 27.96 15.82 20.0 +1 300.0 27.96 15.88 20.0 +1 300.0 27.95 15.95 20.0 +1 300.0 27.92 16.01 20.0 +1 300.0 27.87 16.08 20.0 +1 300.0 27.82 16.16 20.0 +1 300.0 27.74 16.23 20.0 +1 300.0 27.66 16.3 20.0 +1 300.0 27.56 16.38 20.0 +1 300.0 27.45 16.46 20.0 +1 300.0 27.33 16.54 20.0 +1 300.0 27.04 16.71 20.0 +1 300.0 26.88 16.8 20.0 +1 300.0 26.71 16.89 20.0 +1 300.0 26.33 17.07 20.0 +1 300.0 26.12 17.16 20.0 +1 300.0 25.9 17.26 20.0 +1 300.0 25.43 17.45 20.0 +1 300.0 24.39 17.85 20.0 +1 300.0 21.93 18.69 20.0 +1 300.0 21.57 18.81 20.0 +1 300.0 21.21 18.93 20.0 +1 300.0 20.47 19.16 20.0 +1 300.0 18.98 19.64 20.0 +1 300.0 16.04 20.57 20.0 +1 300.0 15.69 20.69 20.0 +1 300.0 15.35 20.8 20.0 +1 300.0 14.68 21.02 20.0 +1 300.0 13.46 21.46 20.0 +1 300.0 13.18 21.56 20.0 +1 300.0 12.9 21.67 20.0 +1 300.0 12.4 21.87 20.0 +1 300.0 12.16 21.97 20.0 +1 300.0 11.94 22.06 20.0 +1 300.0 11.53 22.25 20.0 +1 300.0 11.35 22.34 20.0 +1 300.0 11.18 22.43 20.0 +1 300.0 11.02 22.52 20.0 +1 300.0 10.88 22.6 20.0 +1 300.0 10.76 22.68 20.0 +1 300.0 10.65 22.76 20.0 +1 300.0 10.55 22.84 20.0 +1 300.0 10.48 22.91 20.0 +1 300.0 10.41 22.98 20.0 +1 300.0 10.37 23.05 20.0 +1 300.0 10.34 23.12 20.0 +1 300.0 10.32 23.18 20.0 +1 300.0 10.32 23.24 20.0 +1 300.0 10.34 23.3 20.0 +1 300.0 10.38 23.35 20.0 +1 300.0 10.43 23.4 20.0 +1 300.0 10.5 23.45 20.0 +1 300.0 10.58 23.5 20.0 +1 300.0 10.68 23.54 20.0 +1 300.0 10.79 23.58 20.0 +1 300.0 10.93 23.61 20.0 +1 300.0 11.07 23.64 20.0 +1 300.0 11.23 23.67 20.0 +1 300.0 11.41 23.7 20.0 +1 300.0 11.6 23.72 20.0 +1 300.0 11.81 23.74 20.0 +1 300.0 12.03 23.75 20.0 +1 300.0 12.26 23.76 20.0 +1 300.0 12.51 23.77 20.0 +1 300.0 12.77 23.77 20.0 +1 300.0 13.04 23.77 20.0 +1 300.0 13.32 23.77 20.0 +1 300.0 13.6 23.76 20.0 +1 300.0 13.89 23.75 20.0 +1 300.0 14.18 23.73 20.0 +1 300.0 14.49 23.72 20.0 +1 300.0 14.8 23.7 20.0 +1 300.0 15.12 23.67 20.0 +1 300.0 15.45 23.64 20.0 +1 300.0 15.78 23.61 20.0 +1 300.0 16.13 23.58 20.0 +1 300.0 16.48 23.54 20.0 +1 300.0 16.83 23.5 20.0 +1 300.0 17.19 23.45 20.0 +1 300.0 17.55 23.4 20.0 +1 300.0 17.92 23.35 20.0 +1 300.0 18.67 23.23 20.0 +1 300.0 19.05 23.17 20.0 +1 300.0 19.42 23.1 20.0 +1 300.0 20.19 22.96 20.0 +1 300.0 20.57 22.88 20.0 +1 300.0 20.95 22.8 20.0 +1 300.0 21.71 22.62 20.0 +1 300.0 22.09 22.53 20.0 +1 300.0 22.47 22.43 20.0 +1 300.0 23.21 22.23 20.0 +1 300.0 23.57 22.13 20.0 +1 300.0 23.93 22.02 20.0 +1 300.0 24.64 21.78 20.0 +1 300.0 24.98 21.66 20.0 +1 300.0 25.32 21.54 20.0 +1 300.0 25.98 21.28 20.0 +1 300.0 26.29 21.14 20.0 +1 300.0 26.6 21.01 20.0 +1 300.0 27.19 20.72 20.0 +1 300.0 27.46 20.57 20.0 +1 300.0 27.73 20.42 20.0 +1 300.0 27.99 20.26 20.0 +1 300.0 28.24 20.11 20.0 +1 300.0 28.48 19.94 20.0 +1 300.0 28.7 19.78 20.0 +1 300.0 29.12 19.44 20.0 +1 300.0 29.3 19.27 20.0 +1 300.0 29.48 19.09 20.0 +1 300.0 29.64 18.91 20.0 +1 300.0 29.79 18.72 20.0 +1 300.0 29.93 18.54 20.0 +1 300.0 30.05 18.35 20.0 +1 300.0 30.16 18.16 20.0 +1 300.0 30.26 17.96 20.0 +1 300.0 30.34 17.76 20.0 +1 300.0 30.41 17.56 20.0 +1 300.0 30.46 17.36 20.0 +1 300.0 30.5 17.15 20.0 +1 300.0 30.52 16.94 20.0 +1 300.0 30.53 16.73 20.0 +1 300.0 30.53 16.52 20.0 +1 300.0 30.51 16.3 20.0 +1 300.0 30.47 16.07 20.0 +1 300.0 30.42 15.83 20.0 +1 300.0 30.35 15.58 20.0 +1 300.0 30.27 15.34 20.0 +1 300.0 30.16 15.09 20.0 +1 300.0 30.05 14.84 20.0 +1 300.0 29.91 14.59 20.0 +1 300.0 29.76 14.33 20.0 +1 300.0 29.6 14.07 20.0 +1 300.0 29.42 13.81 20.0 +1 300.0 29.01 13.29 20.0 +1 300.0 28.79 13.02 20.0 +1 300.0 28.55 12.75 20.0 +1 300.0 28.04 12.21 20.0 +1 300.0 27.77 11.94 20.0 +1 300.0 27.48 11.66 20.0 +1 300.0 26.88 11.11 20.0 +1 300.0 25.55 9.98 20.0 +1 300.0 22.57 7.69 20.0 +1 300.0 22.18 7.41 20.0 +1 300.0 21.79 7.12 20.0 +1 300.0 21.0 6.54 20.0 +1 300.0 19.44 5.38 20.0 +1 300.0 19.06 5.09 20.0 +1 300.0 18.68 4.81 20.0 +1 300.0 17.93 4.23 20.0 +1 300.0 16.51 3.1 20.0 +1 300.0 16.18 2.81 20.0 +1 300.0 15.85 2.53 20.0 +1 300.0 15.23 1.97 20.0 +1 300.0 14.93 1.7 20.0 +1 300.0 14.65 1.42 20.0 +1 300.0 14.12 0.88 20.0 +1 300.0 13.87 0.61 20.0 +1 300.0 13.63 0.34 20.0 +1 300.0 13.41 0.07 20.0 +1 300.0 13.2 -0.19 20.0 +1 300.0 13.01 -0.46 20.0 +1 300.0 12.83 -0.72 20.0 +1 300.0 12.67 -0.98 20.0 +1 300.0 12.52 -1.23 20.0 +1 300.0 12.39 -1.47 20.0 +1 300.0 12.28 -1.7 20.0 +1 300.0 12.19 -1.94 20.0 +1 300.0 12.11 -2.17 20.0 +1 300.0 12.04 -2.4 20.0 +1 300.0 11.98 -2.62 20.0 +1 300.0 11.94 -2.84 20.0 +1 300.0 11.92 -3.06 20.0 +1 300.0 11.9 -3.28 20.0 +1 300.0 11.91 -3.5 20.0 +1 300.0 11.93 -3.71 20.0 +1 300.0 11.96 -3.92 20.0 +1 300.0 12.01 -4.13 20.0 +1 300.0 12.07 -4.34 20.0 +1 300.0 12.14 -4.54 20.0 +1 300.0 12.23 -4.74 20.0 +1 300.0 12.34 -4.94 20.0 +1 300.0 12.45 -5.13 20.0 +1 300.0 12.58 -5.32 20.0 +1 300.0 12.73 -5.51 20.0 +1 300.0 12.89 -5.7 20.0 +1 300.0 13.06 -5.88 20.0 +1 300.0 13.24 -6.05 20.0 +1 300.0 13.44 -6.23 20.0 +1 300.0 13.65 -6.4 20.0 +1 300.0 13.87 -6.57 20.0 +1 300.0 14.1 -6.74 20.0 +1 300.0 14.35 -6.9 20.0 +1 300.0 14.6 -7.06 20.0 +1 300.0 14.86 -7.22 20.0 +1 300.0 15.42 -7.52 20.0 +1 300.0 15.72 -7.66 20.0 +1 300.0 16.02 -7.81 20.0 +1 300.0 16.65 -8.08 20.0 +1 300.0 16.98 -8.21 20.0 +1 300.0 17.31 -8.34 20.0 +1 300.0 17.99 -8.59 20.0 +1 300.0 18.34 -8.7 20.0 +1 300.0 18.7 -8.82 20.0 +1 300.0 19.42 -9.04 20.0 +1 300.0 20.9 -9.43 20.0 +1 300.0 21.28 -9.52 20.0 +1 300.0 21.65 -9.6 20.0 +1 300.0 22.4 -9.76 20.0 +1 300.0 22.77 -9.83 20.0 +1 300.0 23.14 -9.9 20.0 +1 300.0 23.87 -10.03 20.0 +1 300.0 24.23 -10.09 20.0 +1 300.0 24.58 -10.15 20.0 +1 300.0 24.94 -10.2 20.0 +1 300.0 25.28 -10.25 20.0 +1 300.0 25.62 -10.29 20.0 +1 300.0 25.96 -10.34 20.0 +1 300.0 26.6 -10.41 20.0 +1 300.0 26.91 -10.44 20.0 +1 300.0 27.21 -10.46 20.0 +1 300.0 27.5 -10.49 20.0 +1 300.0 27.78 -10.51 20.0 +1 300.0 28.05 -10.53 20.0 +1 300.0 28.31 -10.54 20.0 +1 300.0 28.57 -10.55 20.0 +1 300.0 28.81 -10.56 20.0 +1 300.0 29.04 -10.56 20.0 +1 300.0 29.26 -10.56 20.0 +1 300.0 29.47 -10.56 20.0 +1 300.0 29.66 -10.56 20.0 +1 300.0 29.85 -10.55 20.0 +1 300.0 30.02 -10.54 20.0 +1 300.0 30.18 -10.52 20.0 +1 300.0 30.33 -10.5 20.0 +1 300.0 30.46 -10.48 20.0 +1 300.0 30.58 -10.46 20.0 +1 300.0 30.69 -10.44 20.0 +1 300.0 30.78 -10.41 20.0 +1 300.0 30.86 -10.37 20.0 +1 300.0 30.93 -10.34 20.0 +1 300.0 30.98 -10.3 20.0 +1 300.0 31.02 -10.26 20.0 +1 300.0 31.05 -10.22 20.0 +1 300.0 31.06 -10.18 20.0 +1 300.0 31.05 -10.13 20.0 +1 300.0 31.04 -10.08 20.0 +1 300.0 31.01 -10.03 20.0 +1 300.0 30.96 -9.97 20.0 +1 300.0 30.9 -9.91 20.0 +1 300.0 30.83 -9.86 20.0 +1 300.0 30.74 -9.79 20.0 +1 300.0 30.64 -9.73 20.0 +1 300.0 30.52 -9.66 20.0 +1 300.0 30.4 -9.6 20.0 +1 300.0 30.25 -9.53 20.0 +1 300.0 30.1 -9.46 20.0 +1 300.0 29.75 -9.31 20.0 +1 300.0 29.56 -9.23 20.0 +1 300.0 29.36 -9.15 20.0 +1 300.0 28.92 -8.99 20.0 +1 300.0 27.9 -8.64 20.0 +1 300.0 27.62 -8.55 20.0 +1 300.0 27.34 -8.46 20.0 +1 300.0 26.73 -8.28 20.0 +1 300.0 25.44 -7.9 20.0 +1 300.0 22.59 -7.1 20.0 +1 300.0 22.19 -6.99 20.0 +1 300.0 21.79 -6.88 20.0 +1 300.0 20.98 -6.66 20.0 +1 300.0 19.37 -6.22 20.0 +1 300.0 18.98 -6.11 20.0 +1 300.0 18.58 -6.0 20.0 +1 300.0 17.81 -5.78 20.0 +1 300.0 16.35 -5.36 20.0 +1 300.0 16.0 -5.26 20.0 +1 300.0 15.66 -5.16 20.0 +1 300.0 15.02 -4.95 20.0 +1 300.0 14.71 -4.86 20.0 +1 300.0 14.41 -4.76 20.0 +1 300.0 13.85 -4.57 20.0 +1 300.0 13.59 -4.48 20.0 +1 300.0 13.34 -4.39 20.0 +1 300.0 12.88 -4.21 20.0 +1 300.0 12.68 -4.13 20.0 +1 300.0 12.48 -4.04 20.0 +1 300.0 12.14 -3.89 20.0 +1 300.0 11.99 -3.81 20.0 +1 300.0 11.86 -3.74 20.0 +1 300.0 11.74 -3.67 20.0 +1 300.0 11.64 -3.6 20.0 +1 300.0 11.55 -3.53 20.0 +1 300.0 11.49 -3.47 20.0 +1 300.0 11.43 -3.41 20.0 +1 300.0 11.4 -3.35 20.0 +1 300.0 11.38 -3.29 20.0 +1 300.0 11.37 -3.24 20.0 +1 300.0 11.39 -3.19 20.0 +1 300.0 11.41 -3.15 20.0 +1 300.0 11.46 -3.1 20.0 +1 300.0 11.52 -3.06 20.0 +1 300.0 11.6 -3.02 20.0 +1 300.0 11.69 -2.99 20.0 +1 300.0 11.8 -2.96 20.0 +1 300.0 11.93 -2.93 20.0 +1 300.0 12.07 -2.91 20.0 +1 300.0 12.22 -2.89 20.0 +1 300.0 12.39 -2.87 20.0 +1 300.0 12.58 -2.86 20.0 +1 300.0 12.78 -2.85 20.0 +1 300.0 12.99 -2.84 20.0 +1 300.0 13.21 -2.84 20.0 +1 300.0 13.45 -2.84 20.0 +1 300.0 13.7 -2.84 20.0 +1 300.0 13.97 -2.85 20.0 +1 300.0 14.24 -2.86 20.0 +1 300.0 14.53 -2.88 20.0 +1 300.0 14.82 -2.9 20.0 +1 300.0 15.13 -2.92 20.0 +1 300.0 15.43 -2.95 20.0 +1 300.0 15.73 -2.98 20.0 +1 300.0 16.04 -3.01 20.0 +1 300.0 16.35 -3.04 20.0 +1 300.0 16.68 -3.08 20.0 +1 300.0 17.0 -3.12 20.0 +1 300.0 17.68 -3.22 20.0 +1 300.0 18.02 -3.27 20.0 +1 300.0 18.37 -3.33 20.0 +1 300.0 19.07 -3.45 20.0 +1 300.0 19.43 -3.52 20.0 +1 300.0 19.78 -3.59 20.0 +1 300.0 20.5 -3.74 20.0 +1 300.0 20.86 -3.82 20.0 +1 300.0 21.21 -3.91 20.0 +1 300.0 21.93 -4.09 20.0 +1 300.0 22.28 -4.18 20.0 +1 300.0 22.63 -4.28 20.0 +1 300.0 23.31 -4.49 20.0 +1 300.0 23.65 -4.6 20.0 +1 300.0 23.98 -4.71 20.0 +1 300.0 24.63 -4.95 20.0 +1 300.0 24.95 -5.07 20.0 +1 300.0 25.25 -5.2 20.0 +1 300.0 25.85 -5.46 20.0 +1 300.0 26.13 -5.6 20.0 +1 300.0 26.4 -5.74 20.0 +1 300.0 26.67 -5.88 20.0 +1 300.0 26.93 -6.03 20.0 +1 300.0 27.17 -6.18 20.0 +1 300.0 27.41 -6.33 20.0 +1 300.0 27.84 -6.65 20.0 +1 300.0 28.04 -6.81 20.0 +1 300.0 28.23 -6.98 20.0 +1 300.0 28.41 -7.15 20.0 +1 300.0 28.57 -7.32 20.0 +1 300.0 28.73 -7.5 20.0 +1 300.0 28.86 -7.67 20.0 +1 300.0 28.99 -7.86 20.0 +1 300.0 29.1 -8.04 20.0 +1 300.0 29.2 -8.23 20.0 +1 300.0 29.28 -8.42 20.0 +1 300.0 29.35 -8.61 20.0 +1 300.0 29.41 -8.8 20.0 +1 300.0 29.45 -9.0 20.0 +1 300.0 29.48 -9.2 20.0 +1 300.0 29.49 -9.41 20.0 +1 300.0 29.49 -9.61 20.0 +1 300.0 29.48 -9.82 20.0 +1 300.0 29.45 -10.03 20.0 +1 300.0 29.4 -10.24 20.0 +1 300.0 29.34 -10.46 20.0 +1 300.0 29.27 -10.68 20.0 +1 300.0 29.18 -10.9 20.0 +1 300.0 29.08 -11.12 20.0 +1 300.0 28.96 -11.34 20.0 +1 300.0 28.82 -11.59 20.0 +1 300.0 28.67 -11.84 20.0 +1 300.0 28.49 -12.09 20.0 +1 300.0 28.3 -12.34 20.0 +1 300.0 28.1 -12.59 20.0 +1 300.0 27.88 -12.85 20.0 +1 300.0 27.4 -13.37 20.0 +1 300.0 27.14 -13.63 20.0 +1 300.0 26.87 -13.9 20.0 +1 300.0 26.28 -14.43 20.0 +1 300.0 24.97 -15.51 20.0 +1 300.0 24.61 -15.79 20.0 +1 300.0 24.25 -16.06 20.0 +1 300.0 23.5 -16.62 20.0 +1 300.0 21.92 -17.73 20.0 +1 300.0 18.57 -19.98 20.0 +1 300.0 18.15 -20.27 20.0 +1 300.0 17.73 -20.55 20.0 +1 300.0 16.9 -21.11 20.0 +1 300.0 15.29 -22.22 20.0 +1 300.0 14.9 -22.5 20.0 +1 300.0 14.52 -22.77 20.0 +1 300.0 13.78 -23.32 20.0 +1 300.0 12.41 -24.4 20.0 +1 300.0 12.1 -24.67 20.0 +1 300.0 11.79 -24.93 20.0 +1 300.0 11.22 -25.45 20.0 +1 300.0 10.96 -25.71 20.0 +1 300.0 10.71 -25.97 20.0 +1 300.0 10.25 -26.48 20.0 +1 300.0 10.04 -26.73 20.0 +1 300.0 9.85 -26.97 20.0 +1 300.0 9.67 -27.22 20.0 +1 300.0 9.51 -27.46 20.0 +1 300.0 9.36 -27.7 20.0 +1 300.0 9.23 -27.94 20.0 +1 300.0 9.12 -28.18 20.0 +1 300.0 9.02 -28.41 20.0 +1 300.0 8.94 -28.63 20.0 +1 300.0 8.87 -28.86 20.0 +1 300.0 8.83 -29.07 20.0 +1 300.0 8.8 -29.29 20.0 +1 300.0 8.78 -29.5 20.0 +1 300.0 8.78 -29.72 20.0 +1 300.0 8.8 -29.92 20.0 +1 300.0 8.83 -30.13 20.0 +1 300.0 8.88 -30.33 20.0 +1 300.0 8.95 -30.53 20.0 +1 300.0 9.03 -30.72 20.0 +1 300.0 9.13 -30.91 20.0 +1 300.0 9.24 -31.1 20.0 +1 300.0 9.36 -31.29 20.0 +1 300.0 9.51 -31.47 20.0 +1 300.0 9.66 -31.64 20.0 +1 300.0 9.83 -31.82 20.0 +1 300.0 10.02 -31.99 20.0 +1 300.0 10.21 -32.16 20.0 +1 300.0 10.43 -32.32 20.0 +1 300.0 10.65 -32.48 20.0 +1 300.0 10.88 -32.63 20.0 +1 300.0 11.39 -32.93 20.0 +1 300.0 11.66 -33.08 20.0 +1 300.0 11.95 -33.22 20.0 +1 300.0 12.54 -33.48 20.0 +1 300.0 12.85 -33.61 20.0 +1 300.0 13.16 -33.73 20.0 +1 300.0 13.82 -33.97 20.0 +1 300.0 14.16 -34.08 20.0 +1 300.0 14.5 -34.19 20.0 +1 300.0 15.21 -34.39 20.0 +1 300.0 15.57 -34.49 20.0 +1 300.0 15.93 -34.58 20.0 +1 300.0 16.66 -34.75 20.0 +1 300.0 17.03 -34.83 20.0 +1 300.0 17.4 -34.9 20.0 +1 300.0 17.77 -34.97 20.0 +1 300.0 18.13 -35.04 20.0 +1 300.0 18.5 -35.1 20.0 +1 300.0 18.86 -35.16 20.0 +1 300.0 19.59 -35.26 20.0 +1 300.0 19.94 -35.3 20.0 +1 300.0 20.29 -35.35 20.0 +1 300.0 20.63 -35.38 20.0 +1 300.0 20.97 -35.42 20.0 +1 300.0 21.3 -35.44 20.0 +1 300.0 21.63 -35.47 20.0 +1 300.0 21.95 -35.49 20.0 +1 300.0 22.26 -35.51 20.0 +1 300.0 22.56 -35.52 20.0 +1 300.0 22.85 -35.53 20.0 +1 300.0 23.13 -35.53 20.0 +1 300.0 23.4 -35.53 20.0 +1 300.0 23.66 -35.53 20.0 +1 300.0 23.9 -35.52 20.0 +1 300.0 24.14 -35.51 20.0 +1 300.0 24.36 -35.5 20.0 +1 300.0 24.56 -35.48 20.0 +1 300.0 24.75 -35.46 20.0 +1 300.0 24.92 -35.44 20.0 +1 300.0 25.08 -35.41 20.0 +1 300.0 25.23 -35.38 20.0 +1 300.0 25.36 -35.34 20.0 +1 300.0 25.49 -35.31 20.0 +1 300.0 25.6 -35.27 20.0 +1 300.0 25.69 -35.23 20.0 +1 300.0 25.78 -35.18 20.0 +1 300.0 25.85 -35.13 20.0 +1 300.0 25.9 -35.08 20.0 +1 300.0 25.94 -35.03 20.0 +1 300.0 25.97 -34.97 20.0 +1 300.0 25.99 -34.91 20.0 +1 300.0 25.99 -34.85 20.0 +1 300.0 25.97 -34.78 20.0 +1 300.0 25.94 -34.72 20.0 +1 300.0 25.9 -34.65 20.0 +1 300.0 25.84 -34.58 20.0 +1 300.0 25.77 -34.5 20.0 +1 300.0 25.68 -34.42 20.0 +1 300.0 25.59 -34.34 20.0 +1 300.0 25.47 -34.26 20.0 +1 300.0 25.34 -34.17 20.0 +1 300.0 25.2 -34.09 20.0 +1 300.0 24.88 -33.91 20.0 +1 300.0 24.7 -33.81 20.0 +1 300.0 24.51 -33.72 20.0 +1 300.0 24.09 -33.52 20.0 +1 300.0 23.86 -33.42 20.0 +1 300.0 23.61 -33.32 20.0 +1 300.0 23.09 -33.11 20.0 +1 300.0 21.93 -32.67 20.0 +1 300.0 19.2 -31.74 20.0 +1 300.0 12.95 -29.75 20.0 +1 300.0 12.53 -29.61 20.0 +1 300.0 12.12 -29.48 20.0 +1 300.0 11.3 -29.21 20.0 +1 300.0 9.75 -28.68 20.0 +1 300.0 9.38 -28.55 20.0 +1 300.0 9.03 -28.42 20.0 +1 300.0 8.34 -28.17 20.0 +1 300.0 7.1 -27.68 20.0 +1 300.0 6.83 -27.56 20.0 +1 300.0 6.56 -27.44 20.0 +1 300.0 6.07 -27.21 20.0 +1 300.0 5.85 -27.1 20.0 +1 300.0 5.64 -26.99 20.0 +1 300.0 5.27 -26.78 20.0 +1 300.0 5.11 -26.68 20.0 +1 300.0 4.96 -26.58 20.0 +1 300.0 4.83 -26.48 20.0 +1 300.0 4.72 -26.38 20.0 +1 300.0 4.62 -26.29 20.0 +1 300.0 4.54 -26.2 20.0 +1 300.0 4.48 -26.11 20.0 +1 300.0 4.43 -26.03 20.0 +1 300.0 4.4 -25.94 20.0 +1 300.0 4.38 -25.86 20.0 +1 300.0 4.38 -25.79 20.0 +1 300.0 4.4 -25.71 20.0 +1 300.0 4.44 -25.64 20.0 +1 300.0 4.49 -25.58 20.0 +1 300.0 4.56 -25.51 20.0 +1 300.0 4.64 -25.45 20.0 +1 300.0 4.74 -25.39 20.0 +1 300.0 4.85 -25.34 20.0 +1 300.0 4.98 -25.29 20.0 +1 300.0 5.13 -25.24 20.0 +1 300.0 5.29 -25.19 20.0 +1 300.0 5.46 -25.15 20.0 +1 300.0 5.65 -25.11 20.0 +1 300.0 5.85 -25.08 20.0 +1 300.0 6.06 -25.05 20.0 +1 300.0 6.29 -25.02 20.0 +1 300.0 6.53 -25.0 20.0 +1 300.0 6.78 -24.98 20.0 +1 300.0 7.04 -24.96 20.0 +1 300.0 7.31 -24.95 20.0 +1 300.0 7.6 -24.94 20.0 +1 300.0 7.89 -24.94 20.0 +1 300.0 8.19 -24.94 20.0 +1 300.0 8.5 -24.94 20.0 +1 300.0 8.82 -24.95 20.0 +1 300.0 9.15 -24.96 20.0 +1 300.0 9.48 -24.98 20.0 +1 300.0 9.82 -24.99 20.0 +1 300.0 10.16 -25.02 20.0 +1 300.0 10.51 -25.04 20.0 +1 300.0 10.84 -25.07 20.0 +1 300.0 11.17 -25.1 20.0 +1 300.0 11.51 -25.14 20.0 +1 300.0 11.84 -25.18 20.0 +1 300.0 12.18 -25.22 20.0 +1 300.0 12.52 -25.27 20.0 +1 300.0 13.2 -25.37 20.0 +1 300.0 13.53 -25.43 20.0 +1 300.0 13.87 -25.49 20.0 +1 300.0 14.2 -25.55 20.0 +1 300.0 14.54 -25.62 20.0 +1 300.0 14.86 -25.69 20.0 +1 300.0 15.19 -25.76 20.0 +1 300.0 15.82 -25.92 20.0 +1 300.0 16.13 -26.0 20.0 +1 300.0 16.44 -26.09 20.0 +1 300.0 16.74 -26.18 20.0 +1 300.0 17.03 -26.28 20.0 +1 300.0 17.31 -26.38 20.0 +1 300.0 17.59 -26.48 20.0 +1 300.0 18.11 -26.69 20.0 +1 300.0 18.36 -26.8 20.0 +1 300.0 18.6 -26.91 20.0 +1 300.0 18.83 -27.03 20.0 +1 300.0 19.05 -27.15 20.0 +1 300.0 19.26 -27.28 20.0 +1 300.0 19.46 -27.4 20.0 +1 300.0 19.64 -27.53 20.0 +1 300.0 19.82 -27.67 20.0 +1 300.0 19.98 -27.8 20.0 +1 300.0 20.13 -27.94 20.0 +1 300.0 20.26 -28.09 20.0 +1 300.0 20.39 -28.23 20.0 +1 300.0 20.5 -28.38 20.0 +1 300.0 20.59 -28.53 20.0 +1 300.0 20.68 -28.68 20.0 +1 300.0 20.75 -28.84 20.0 +1 300.0 20.8 -29.0 20.0 +1 300.0 20.84 -29.16 20.0 +1 300.0 20.87 -29.33 20.0 +1 300.0 20.88 -29.49 20.0 +1 300.0 20.88 -29.66 20.0 +1 300.0 20.86 -29.84 20.0 +1 300.0 20.83 -30.01 20.0 +1 300.0 20.78 -30.19 20.0 +1 300.0 20.72 -30.37 20.0 +1 300.0 20.64 -30.55 20.0 +1 300.0 20.56 -30.73 20.0 +1 300.0 20.45 -30.92 20.0 +1 300.0 20.33 -31.11 20.0 +1 300.0 20.2 -31.3 20.0 +1 300.0 20.05 -31.49 20.0 +1 300.0 19.89 -31.69 20.0 +1 300.0 19.72 -31.89 20.0 +1 300.0 19.53 -32.08 20.0 +1 300.0 19.12 -32.49 20.0 +1 300.0 18.89 -32.69 20.0 +1 300.0 18.66 -32.9 20.0 +1 300.0 18.14 -33.31 20.0 +1 300.0 17.87 -33.52 20.0 +1 300.0 17.59 -33.72 20.0 +1 300.0 17.01 -34.14 20.0 +1 300.0 15.72 -34.99 20.0 +1 300.0 12.8 -36.73 20.0 +1 300.0 12.41 -36.95 20.0 +1 300.0 12.02 -37.17 20.0 +1 300.0 11.22 -37.61 20.0 +1 300.0 9.62 -38.49 20.0 +1 300.0 6.46 -40.24 20.0 +1 300.0 6.07 -40.45 20.0 +1 300.0 5.7 -40.67 20.0 +1 300.0 4.96 -41.1 20.0 +1 300.0 3.57 -41.94 20.0 +1 300.0 3.24 -42.15 20.0 +1 300.0 2.92 -42.36 20.0 +1 300.0 2.3 -42.77 20.0 +1 300.0 1.19 -43.57 20.0 +1 300.0 0.94 -43.76 20.0 +1 300.0 0.71 -43.96 20.0 +1 300.0 0.26 -44.34 20.0 +1 300.0 0.06 -44.53 20.0 +1 300.0 -0.13 -44.72 20.0 +1 300.0 -0.47 -45.09 20.0 +1 300.0 -0.62 -45.27 20.0 +1 300.0 -0.76 -45.44 20.0 +1 300.0 -0.88 -45.62 20.0 +1 300.0 -0.99 -45.79 20.0 +1 300.0 -1.09 -45.97 20.0 +1 300.0 -1.17 -46.13 20.0 +1 300.0 -1.24 -46.3 20.0 +1 300.0 -1.3 -46.47 20.0 +1 300.0 -1.34 -46.64 20.0 +1 300.0 -1.37 -46.81 20.0 +1 300.0 -1.38 -46.98 20.0 +1 300.0 -1.38 -47.15 20.0 +1 300.0 -1.36 -47.31 20.0 +1 300.0 -1.32 -47.47 20.0 +1 300.0 -1.27 -47.62 20.0 +1 300.0 -1.2 -47.77 20.0 +1 300.0 -1.11 -47.92 20.0 +1 300.0 -1.01 -48.07 20.0 +1 300.0 -0.9 -48.21 20.0 +1 300.0 -0.77 -48.35 20.0 +1 300.0 -0.63 -48.48 20.0 +1 300.0 -0.47 -48.61 20.0 +1 300.0 -0.29 -48.74 20.0 +1 300.0 -0.11 -48.86 20.0 +1 300.0 0.09 -48.98 20.0 +1 300.0 0.3 -49.1 20.0 +1 300.0 0.53 -49.21 20.0 +1 300.0 0.77 -49.31 20.0 +1 300.0 1.01 -49.42 20.0 +1 300.0 1.27 -49.52 20.0 +1 300.0 1.82 -49.7 20.0 +1 300.0 2.11 -49.79 20.0 +1 300.0 2.41 -49.88 20.0 +1 300.0 2.71 -49.95 20.0 +1 300.0 3.03 -50.03 20.0 +1 300.0 3.35 -50.1 20.0 +1 300.0 3.67 -50.17 20.0 +1 300.0 4.0 -50.23 20.0 +1 300.0 4.34 -50.29 20.0 +1 300.0 4.68 -50.34 20.0 +1 300.0 5.03 -50.39 20.0 +1 300.0 5.38 -50.44 20.0 +1 300.0 5.73 -50.48 20.0 +1 300.0 6.08 -50.51 20.0 +1 300.0 6.43 -50.55 20.0 +1 300.0 6.79 -50.57 20.0 +1 300.0 7.14 -50.6 20.0 +1 300.0 7.49 -50.62 20.0 +1 300.0 7.84 -50.63 20.0 +1 300.0 8.19 -50.64 20.0 +1 300.0 8.54 -50.65 20.0 +1 300.0 8.88 -50.65 20.0 +1 300.0 9.22 -50.65 20.0 +1 300.0 9.55 -50.65 20.0 +1 300.0 9.88 -50.63 20.0 +1 300.0 10.2 -50.62 20.0 +1 300.0 10.52 -50.6 20.0 +1 300.0 10.82 -50.58 20.0 +1 300.0 11.12 -50.55 20.0 +1 300.0 11.41 -50.52 20.0 +1 300.0 11.69 -50.49 20.0 +1 300.0 11.96 -50.45 20.0 +1 300.0 12.22 -50.4 20.0 +1 300.0 12.47 -50.36 20.0 +1 300.0 12.71 -50.3 20.0 +1 300.0 12.94 -50.25 20.0 +1 300.0 13.15 -50.19 20.0 +1 300.0 13.36 -50.13 20.0 +1 300.0 13.55 -50.06 20.0 +1 300.0 13.72 -49.99 20.0 +1 300.0 13.88 -49.92 20.0 +1 300.0 14.02 -49.84 20.0 +1 300.0 14.14 -49.77 20.0 +1 300.0 14.26 -49.69 20.0 +1 300.0 14.36 -49.6 20.0 +1 300.0 14.44 -49.52 20.0 +1 300.0 14.51 -49.43 20.0 +1 300.0 14.57 -49.34 20.0 +1 300.0 14.62 -49.24 20.0 +1 300.0 14.64 -49.15 20.0 +1 300.0 14.66 -49.05 20.0 +1 300.0 14.66 -48.94 20.0 +1 300.0 14.65 -48.84 20.0 +1 300.0 14.63 -48.73 20.0 +1 300.0 14.58 -48.62 20.0 +1 300.0 14.53 -48.5 20.0 +1 300.0 14.46 -48.39 20.0 +1 300.0 14.38 -48.27 20.0 +1 300.0 14.28 -48.15 20.0 +1 300.0 14.17 -48.02 20.0 +1 300.0 14.04 -47.9 20.0 +1 300.0 13.9 -47.77 20.0 +1 300.0 13.75 -47.63 20.0 +1 300.0 13.4 -47.37 20.0 +1 300.0 13.21 -47.23 20.0 +1 300.0 13.0 -47.09 20.0 +1 300.0 12.55 -46.8 20.0 +1 300.0 11.5 -46.21 20.0 +1 300.0 11.21 -46.06 20.0 +1 300.0 10.92 -45.9 20.0 +1 300.0 10.29 -45.59 20.0 +1 300.0 8.92 -44.95 20.0 +1 300.0 5.87 -43.61 20.0 +1 300.0 5.47 -43.44 20.0 +1 300.0 5.06 -43.27 20.0 +1 300.0 4.25 -42.93 20.0 +1 300.0 2.6 -42.24 20.0 +1 300.0 -0.6 -40.87 20.0 +1 300.0 -1.02 -40.68 20.0 +1 300.0 -1.44 -40.49 20.0 +1 300.0 -2.26 -40.12 20.0 +1 300.0 -3.76 -39.38 20.0 +1 300.0 -4.11 -39.2 20.0 +1 300.0 -4.45 -39.02 20.0 +1 300.0 -5.08 -38.66 20.0 +1 300.0 -5.38 -38.49 20.0 +1 300.0 -5.66 -38.32 20.0 +1 300.0 -6.18 -37.98 20.0 +1 300.0 -6.42 -37.81 20.0 +1 300.0 -6.64 -37.65 20.0 +1 300.0 -6.84 -37.48 20.0 +1 300.0 -7.03 -37.32 20.0 +1 300.0 -7.2 -37.16 20.0 +1 300.0 -7.35 -37.01 20.0 +1 300.0 -7.49 -36.86 20.0 +1 300.0 -7.61 -36.71 20.0 +1 300.0 -7.71 -36.56 20.0 +1 300.0 -7.79 -36.41 20.0 +1 300.0 -7.86 -36.27 20.0 +1 300.0 -7.9 -36.13 20.0 +1 300.0 -7.93 -36.0 20.0 +1 300.0 -7.95 -35.86 20.0 +1 300.0 -7.94 -35.73 20.0 +1 300.0 -7.92 -35.61 20.0 +1 300.0 -7.88 -35.48 20.0 +1 300.0 -7.82 -35.36 20.0 +1 300.0 -7.74 -35.25 20.0 +1 300.0 -7.65 -35.13 20.0 +1 300.0 -7.54 -35.02 20.0 +1 300.0 -7.42 -34.92 20.0 +1 300.0 -7.28 -34.81 20.0 +1 300.0 -7.12 -34.72 20.0 +1 300.0 -6.95 -34.62 20.0 +1 300.0 -6.76 -34.53 20.0 +1 300.0 -6.56 -34.44 20.0 +1 300.0 -6.34 -34.36 20.0 +1 300.0 -6.12 -34.28 20.0 +1 300.0 -5.87 -34.2 20.0 +1 300.0 -5.62 -34.13 20.0 +1 300.0 -5.35 -34.06 20.0 +1 300.0 -5.08 -33.99 20.0 +1 300.0 -4.79 -33.93 20.0 +1 300.0 -4.49 -33.88 20.0 +1 300.0 -4.18 -33.82 20.0 +1 300.0 -3.86 -33.77 20.0 +1 300.0 -3.54 -33.73 20.0 +1 300.0 -3.21 -33.69 20.0 +1 300.0 -2.87 -33.66 20.0 +1 300.0 -2.52 -33.62 20.0 +1 300.0 -2.17 -33.6 20.0 +1 300.0 -1.82 -33.57 20.0 +1 300.0 -1.46 -33.55 20.0 +1 300.0 -1.1 -33.54 20.0 +1 300.0 -0.73 -33.53 20.0 +1 300.0 -0.37 -33.52 20.0 +1 300.0 0.0 -33.52 20.0 +0 300.0 0.0 -33.52 6.0 +0 300.0 37.56 12.33 6.0 +1 300.0 37.56 12.33 -1.0 +1 300.0 37.56 0.88 -1.0 +1 300.0 49.01 0.88 -1.0 +1 300.0 49.01 12.33 -1.0 +1 300.0 37.56 12.33 -1.0 +0 300.0 37.56 12.33 6.0 +0 300.0 37.56 0.88 6.0 +1 300.0 37.56 0.88 -1.0 +1 300.0 37.56 -10.57 -1.0 +1 300.0 49.01 -10.57 -1.0 +1 300.0 49.01 0.88 -1.0 +1 300.0 37.56 0.88 -1.0 +0 300.0 37.56 0.88 6.0 +0 300.0 49.01 12.33 6.0 +1 300.0 49.01 12.33 -1.0 +1 300.0 52.08 15.01 -1.0 +0 300.0 52.08 15.01 6.0 +0 300.0 49.01 0.88 6.0 +1 300.0 49.01 0.88 -1.0 +1 300.0 52.08 6.21 -1.0 +1 300.0 52.08 15.01 -1.0 +1 300.0 43.29 15.01 -1.0 +1 300.0 37.56 12.33 -1.0 +0 300.0 37.56 12.33 6.0 +0 300.0 49.01 -10.57 6.0 +1 300.0 49.01 -10.57 -1.0 +1 300.0 52.08 -2.58 -1.0 +1 300.0 52.08 6.21 -1.0 +1 300.0 49.01 0.88 -1.0 +0 300.0 49.01 0.88 6.0 +0 300.0 49.01 0.88 20.0 +0 300.0 0.0 0.0 20.0 +0 300.0 0.0 0.0 20.0 diff --git a/doc/test/matlab_convert.py b/doc/test/matlab_convert.py new file mode 100755 index 0000000..cf750ac --- /dev/null +++ b/doc/test/matlab_convert.py @@ -0,0 +1,270 @@ +#!/usr/bin/env python +"""\ +The MIT License (MIT) + +Copyright (c) 2014 Sungeun K. Jeon + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +---------------------------------------------------------------------------------------- +""" +"""\ +G-code preprocessor for the grbl_sim.m MATLAB script. Parses the g-code program to a +specific file format for the MATLAB script to use. Based on PreGrbl by @chamnit. + +How to use: When running this python script, it will process the g-code program under +the filename "test.gcode" (may be changed below) and produces a file called "matlab.gcode" +that the grbl_sim.m MATLAB script will search for and execute. +""" + +import re +from math import * +from copy import * + +# -= SETTINGS =- +filein = 'test.gcode' # Input file name +fileout = 'matlab.gcode' # Output file name +ndigits_in = 4 # inch significant digits after '.' +ndigits_mm = 2 # mm significant digits after '.' +# mm_per_arc_segment = 0.38 # mm per arc segment +arc_tolerance = 0.00005*25.4 +n_arc_correction = 20 +inch2mm = 25.4 # inch to mm conversion scalar +verbose = False # Verbose flag to show all progress +remove_unsupported = True # Removal flag for all unsupported statements + +# Initialize parser state +gc = { 'current_xyz' : [0,0,0], + 'feed_rate' : 0, # F0 + 'motion_mode' : 'SEEK', # G00 + 'plane_axis' : [0,1,2], # G17 + 'inches_mode' : False, # G21 + 'inverse_feedrate_mode' : False, # G94 + 'absolute_mode' : True} # G90 + +def unit_conv(val) : # Converts value to mm + if gc['inches_mode'] : val *= inch2mm + return(val) + +def fout_conv(val) : # Returns converted value as rounded string for output file. + if gc['inches_mode'] : return( str(round(val/inch2mm,ndigits_in)) ) + else : return( str(round(val,ndigits_mm)) ) + +# Open g-code file +fin = open(filein,'r'); +fout = open(fileout,'w'); + +# Iterate through g-code file +l_count = 0 +for line in fin: + l_count += 1 # Iterate line counter + + # Strip comments/spaces/tabs/new line and capitalize. Comment MSG not supported. + block = re.sub('\s|\(.*?\)','',line).upper() + block = re.sub('\\\\','',block) # Strip \ block delete character + block = re.sub('%','',block) # Strip % program start/stop character + + if len(block) == 0 : # Ignore empty blocks + + print "Skipping: " + line.strip() + + else : # Process valid g-code clean block. Assumes no block delete characters or comments + + g_cmd = re.findall(r'[^0-9\.\-]+',block) # Extract block command characters + g_num = re.findall(r'[0-9\.\-]+',block) # Extract block numbers + + # G-code block error checks + # if len(g_cmd) != len(g_num) : print block; raise Exception('Invalid block. Unbalanced word and values.') + if 'N' in g_cmd : + if g_cmd[0]!='N': raise Exception('Line number must be first command in line.') + if g_cmd.count('N') > 1: raise Exception('More than one line number in block.') + g_cmd = g_cmd[1:] # Remove line number word + g_num = g_num[1:] + # Block item repeat checks? (0<=n'M'<5, G/M modal groups) + + # Initialize block state + blk = { 'next_action' : 'DEFAULT', + 'absolute_override' : False, + 'target_xyz' : deepcopy(gc['current_xyz']), + 'offset_ijk' : [0,0,0], + 'radius_mode' : False, + 'unsupported': [] } + + # Pass 1 + for cmd,num in zip(g_cmd,g_num) : + fnum = float(num) + inum = int(fnum) + if cmd is 'G' : + if inum is 0 : gc['motion_mode'] = 'SEEK' + elif inum is 1 : gc['motion_mode'] = 'LINEAR' + elif inum is 2 : gc['motion_mode'] = 'CW_ARC' + elif inum is 3 : gc['motion_mode'] = 'CCW_ARC' + elif inum is 4 : blk['next_action'] = 'DWELL' + elif inum is 17 : gc['plane_axis'] = [0,1,2] # Select XY Plane + elif inum is 18 : gc['plane_axis'] = [0,2,1] # Select XZ Plane + elif inum is 19 : gc['plane_axis'] = [1,2,0] # Select YZ Plane + elif inum is 20 : gc['inches_mode'] = True + elif inum is 21 : gc['inches_mode'] = False + elif inum in [28,30] : blk['next_action'] = 'GO_HOME' + elif inum is 53 : blk['absolute_override'] = True + elif inum is 54 : pass + elif inum is 80 : gc['motion_mode'] = 'MOTION_CANCEL' + elif inum is 90 : gc['absolute_mode'] = True + elif inum is 91 : gc['absolute_mode'] = False + elif inum is 92 : blk['next_action'] = 'SET_OFFSET' + elif inum is 93 : gc['inverse_feedrate_mode'] = True + elif inum is 94 : gc['inverse_feedrate_mode'] = False + else : + print 'Unsupported command ' + cmd + num + ' on line ' + str(l_count) + if remove_unsupported : blk['unsupported'].append(zip(g_cmd,g_num).index((cmd,num))) + elif cmd is 'M' : + if inum in [0,1] : pass # Program Pause + elif inum in [2,30,60] : pass # Program Completed + elif inum is 3 : pass # Spindle Direction 1 + elif inum is 4 : pass # Spindle Direction -1 + elif inum is 5 : pass # Spindle Direction 0 + else : + print 'Unsupported command ' + cmd + num + ' on line ' + str(l_count) + if remove_unsupported : blk['unsupported'].append(zip(g_cmd,g_num).index((cmd,num))) + elif cmd is 'T' : pass # Tool Number + + # Pass 2 + for cmd,num in zip(g_cmd,g_num) : + fnum = float(num) + if cmd is 'F' : gc['feed_rate'] = unit_conv(fnum) # Feed Rate + elif cmd in ['I','J','K'] : blk['offset_ijk'][ord(cmd)-ord('I')] = unit_conv(fnum) # Arc Center Offset + elif cmd is 'N' : pass + elif cmd is 'P' : p = fnum # Misc value parameter + elif cmd is 'R' : r = unit_conv(fnum); blk['radius_mode'] = True # Arc Radius Mode + elif cmd is 'S' : pass # Spindle Speed + elif cmd in ['X','Y','Z'] : # Target Coordinates + if (gc['absolute_mode'] | blk['absolute_override']) : + blk['target_xyz'][ord(cmd)-ord('X')] = unit_conv(fnum) + else : + blk['target_xyz'][ord(cmd)-ord('X')] += unit_conv(fnum) + + # Execute actions + if blk['next_action'] is 'GO_HOME' : + gc['current_xyz'] = deepcopy(blk['target_xyz']) # Update position + elif blk['next_action'] is 'SET_OFFSET' : + pass + elif blk['next_action'] is 'DWELL' : + if p < 0 : raise Exception('Dwell time negative.') + else : # 'DEFAULT' + if gc['motion_mode'] is 'SEEK' : + fout.write('0 '+fout_conv(gc['feed_rate'])) + fout.write(' '+fout_conv(blk['target_xyz'][0])) + fout.write(' '+fout_conv(blk['target_xyz'][1])) + fout.write(' '+fout_conv(blk['target_xyz'][2])) + fout.write('\n') + gc['current_xyz'] = deepcopy(blk['target_xyz']) # Update position + elif gc['motion_mode'] is 'LINEAR' : + fout.write('1 '+fout_conv(gc['feed_rate'])) + fout.write(' '+fout_conv(blk['target_xyz'][0])) + fout.write(' '+fout_conv(blk['target_xyz'][1])) + fout.write(' '+fout_conv(blk['target_xyz'][2])) + fout.write('\n') + gc['current_xyz'] = deepcopy(blk['target_xyz']) # Update position + elif gc['motion_mode'] in ['CW_ARC','CCW_ARC'] : + axis = gc['plane_axis'] + + # Convert radius mode to ijk mode + if blk['radius_mode'] : + x = blk['target_xyz'][axis[0]]-gc['current_xyz'][axis[0]] + y = blk['target_xyz'][axis[1]]-gc['current_xyz'][axis[1]] + if not (x==0 and y==0) : raise Exception('Same target and current XYZ not allowed in arc radius mode.') + h_x2_div_d = -sqrt(4 * r*r - x*x - y*y)/hypot(x,y) + if isnan(h_x2_div_d) : raise Exception('Floating point error in arc conversion') + if gc['motion_mode'] is 'CCW_ARC' : h_x2_div_d = -h_x2_div_d + if r < 0 : h_x2_div_d = -h_x2_div_d + blk['offset_ijk'][axis[0]] = (x-(y*h_x2_div_d))/2; + blk['offset_ijk'][axis[1]] = (y+(x*h_x2_div_d))/2; + else : + radius = sqrt(blk['offset_ijk'][axis[0]]**2+blk['offset_ijk'][axis[1]]**2) + + center_axis0 = gc['current_xyz'][axis[0]]+blk['offset_ijk'][axis[0]] + center_axis1 = gc['current_xyz'][axis[1]]+blk['offset_ijk'][axis[1]] + linear_travel = blk['target_xyz'][axis[2]]-gc['current_xyz'][axis[2]] + r_axis0 = -blk['offset_ijk'][axis[0]] + r_axis1 = -blk['offset_ijk'][axis[1]] + rt_axis0 = blk['target_xyz'][axis[0]] - center_axis0; + rt_axis1 = blk['target_xyz'][axis[1]] - center_axis1; + clockwise_sign = 1 + if gc['motion_mode'] is 'CW_ARC' : clockwise_sign = -1 + + angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1) + if gc['motion_mode'] is 'CW_ARC' : + if angular_travel >= 0 : + angular_travel -= 2*pi + else : + if angular_travel <= 0 : + angular_travel += 2*pi + + millimeters_of_travel = sqrt((angular_travel*radius)**2 + abs(linear_travel)**2) + + mm_per_arc_segment = sqrt(4*(2*radius*arc_tolerance-arc_tolerance**2)) + segments = int(millimeters_of_travel/mm_per_arc_segment) + print segments + print l_count + theta_per_segment = angular_travel/segments + linear_per_segment = linear_travel/segments + cos_T = 1-0.5*theta_per_segment*theta_per_segment + sin_T = theta_per_segment-theta_per_segment**3/6 + print(fout_conv(mm_per_arc_segment)) + print theta_per_segment*180/pi + + arc_target = [0,0,0] + arc_target[axis[2]] = gc['current_xyz'][axis[2]] + + count = 0 + for i in range(1,segments+1) : + if i < segments : + if count < n_arc_correction : + r_axisi = r_axis0*sin_T + r_axis1*cos_T + r_axis0 = r_axis0*cos_T - r_axis1*sin_T + r_axis1 = deepcopy(r_axisi) + count += 1 + else : + cos_Ti = cos((i-1)*theta_per_segment) + sin_Ti = sin((i-1)*theta_per_segment) + print n_arc_correction*(r_axis0 -( -blk['offset_ijk'][axis[0]]*cos_Ti + blk['offset_ijk'][axis[1]]*sin_Ti)) + print n_arc_correction*(r_axis1 -( -blk['offset_ijk'][axis[0]]*sin_Ti - blk['offset_ijk'][axis[1]]*cos_Ti)) + cos_Ti = cos(i*theta_per_segment) + sin_Ti = sin(i*theta_per_segment) + r_axis0 = -blk['offset_ijk'][axis[0]]*cos_Ti + blk['offset_ijk'][axis[1]]*sin_Ti + r_axis1 = -blk['offset_ijk'][axis[0]]*sin_Ti - blk['offset_ijk'][axis[1]]*cos_Ti + count = 0 + arc_target[axis[0]] = center_axis0 + r_axis0 + arc_target[axis[1]] = center_axis1 + r_axis1 + arc_target[axis[2]] += linear_per_segment + else : + arc_target = deepcopy(blk['target_xyz']) # Last segment at target_xyz + # Write only changed variables. + fout.write('1 '+fout_conv(gc['feed_rate'])) + fout.write(' '+fout_conv(arc_target[0])) + fout.write(' '+fout_conv(arc_target[1])) + fout.write(' '+fout_conv(arc_target[2])) + fout.write('\n') + gc['current_xyz'] = deepcopy(arc_target) # Update position + + +print 'Done!' + +# Close files +fin.close() +fout.close() \ No newline at end of file diff --git a/doc/test/test.gcode b/doc/test/test.gcode new file mode 100644 index 0000000..3d9d56c --- /dev/null +++ b/doc/test/test.gcode @@ -0,0 +1,2363 @@ +(Braid.NC Test Program) +T1M6 +G17G21 +G0X0Y0 +G0Z6.000 +G0X37.560Y12.327Z6.000 +G1Z-1.000 F300 +G1Y0.876 +X49.011 +Y12.327 +X37.560 +G0Z6.000 +G0Y0.876 +G1Z-1.000 +G1Y-10.575 +X49.011 +Y0.876 +X37.560 +G0Z6.000 +G0X49.011Y12.327 +G1Z-1.000 +G1X52.084Y15.011 +G0Z6.000 +G0X49.011Y0.876 +G1Z-1.000 +G1X52.084Y6.213 +Y15.011 +X43.286 +X37.560Y12.327 +G0Z6.000 +G0X49.011Y-10.575 +G1Z-1.000 +G1X52.084Y-2.585 +Y6.213 +X49.011Y0.876 +G0Z6.000 +G0Z20.000 +G0X0.000Y0.000 +G1X-7.098Y-39.876 +X-10.296Y-38.118 +X-10.698Y-37.897 +X-11.099Y-37.676 +X-11.894Y-37.235 +X-13.448Y-36.356 +X-13.825Y-36.137 +X-14.197Y-35.919 +X-14.923Y-35.485 +X-16.288Y-34.625 +X-16.636Y-34.395 +X-16.973Y-34.165 +X-17.613Y-33.711 +X-17.915Y-33.486 +X-18.204Y-33.262 +X-18.743Y-32.821 +X-18.992Y-32.602 +X-19.227Y-32.386 +X-19.447Y-32.171 +X-19.652Y-31.959 +X-19.843Y-31.749 +X-20.017Y-31.540 +X-20.176Y-31.335 +X-20.320Y-31.131 +X-20.447Y-30.930 +X-20.558Y-30.731 +X-20.653Y-30.535 +X-20.731Y-30.341 +X-20.793Y-30.150 +X-20.838Y-29.961 +X-20.867Y-29.776 +X-20.879Y-29.593 +X-20.875Y-29.413 +X-20.854Y-29.236 +X-20.817Y-29.062 +X-20.764Y-28.891 +X-20.695Y-28.723 +X-20.610Y-28.559 +X-20.510Y-28.397 +X-20.394Y-28.239 +X-20.263Y-28.084 +X-20.117Y-27.932 +X-19.957Y-27.784 +X-19.782Y-27.639 +X-19.594Y-27.498 +X-19.392Y-27.360 +X-19.177Y-27.226 +X-18.950Y-27.095 +X-18.710Y-26.968 +X-18.459Y-26.845 +X-18.197Y-26.725 +X-17.924Y-26.609 +X-17.641Y-26.497 +X-17.349Y-26.389 +X-16.737Y-26.183 +X-16.419Y-26.086 +X-16.094Y-25.993 +X-15.763Y-25.904 +X-15.426Y-25.818 +X-15.083Y-25.737 +X-14.736Y-25.660 +X-14.030Y-25.516 +X-13.672Y-25.451 +X-13.313Y-25.389 +X-12.953Y-25.332 +X-12.591Y-25.278 +X-12.230Y-25.228 +X-11.870Y-25.182 +X-11.511Y-25.140 +X-11.155Y-25.103 +X-10.824Y-25.071 +X-10.497Y-25.043 +X-10.173Y-25.018 +X-9.853Y-24.996 +X-9.538Y-24.978 +X-9.228Y-24.964 +X-8.924Y-24.952 +X-8.625Y-24.944 +X-8.334Y-24.940 +X-8.049Y-24.939 +X-7.772Y-24.941 +X-7.504Y-24.946 +X-7.243Y-24.955 +X-6.992Y-24.967 +X-6.750Y-24.982 +X-6.518Y-25.000 +X-6.295Y-25.022 +X-6.084Y-25.047 +X-5.883Y-25.075 +X-5.694Y-25.106 +X-5.517Y-25.140 +X-5.351Y-25.177 +X-5.197Y-25.217 +X-5.057Y-25.260 +X-4.928Y-25.306 +X-4.813Y-25.355 +X-4.711Y-25.407 +X-4.623Y-25.461 +X-4.548Y-25.518 +X-4.487Y-25.578 +X-4.439Y-25.641 +X-4.406Y-25.707 +X-4.387Y-25.775 +X-4.382Y-25.845 +X-4.392Y-25.918 +X-4.416Y-25.994 +X-4.454Y-26.072 +X-4.506Y-26.152 +X-4.572Y-26.235 +X-4.653Y-26.320 +X-4.748Y-26.407 +X-4.857Y-26.496 +X-4.980Y-26.588 +X-5.117Y-26.681 +X-5.267Y-26.777 +X-5.431Y-26.874 +X-5.798Y-27.074 +X-6.001Y-27.177 +X-6.216Y-27.282 +X-6.683Y-27.496 +X-7.755Y-27.942 +X-8.049Y-28.057 +X-8.353Y-28.173 +X-8.990Y-28.409 +X-10.362Y-28.891 +X-10.753Y-29.024 +X-11.151Y-29.158 +X-11.968Y-29.428 +X-13.658Y-29.973 +X-17.113Y-31.067 +X-17.538Y-31.202 +X-17.959Y-31.337 +X-18.788Y-31.604 +X-20.370Y-32.127 +X-20.746Y-32.255 +X-21.113Y-32.382 +X-21.817Y-32.631 +X-23.093Y-33.111 +X-23.381Y-33.226 +X-23.656Y-33.339 +X-24.164Y-33.560 +X-24.397Y-33.667 +X-24.615Y-33.772 +X-25.005Y-33.974 +X-25.177Y-34.072 +X-25.332Y-34.167 +X-25.472Y-34.259 +X-25.595Y-34.349 +X-25.701Y-34.436 +X-25.791Y-34.520 +X-25.864Y-34.601 +X-25.920Y-34.679 +X-25.960Y-34.754 +X-25.982Y-34.826 +X-25.987Y-34.895 +X-25.976Y-34.961 +X-25.948Y-35.023 +X-25.903Y-35.082 +X-25.841Y-35.138 +X-25.763Y-35.190 +X-25.669Y-35.239 +X-25.559Y-35.284 +X-25.433Y-35.325 +X-25.291Y-35.363 +X-25.134Y-35.397 +X-24.963Y-35.428 +X-24.777Y-35.455 +X-24.577Y-35.477 +X-24.367Y-35.496 +X-24.144Y-35.511 +X-23.909Y-35.522 +X-23.661Y-35.529 +X-23.403Y-35.532 +X-23.133Y-35.532 +X-22.853Y-35.527 +X-22.563Y-35.519 +X-22.263Y-35.506 +X-21.955Y-35.490 +X-21.638Y-35.470 +X-21.313Y-35.445 +X-20.981Y-35.416 +X-20.643Y-35.384 +X-20.299Y-35.347 +X-19.949Y-35.306 +X-19.595Y-35.261 +X-19.237Y-35.212 +X-18.875Y-35.158 +X-18.511Y-35.101 +X-18.145Y-35.039 +X-17.777Y-34.973 +X-17.041Y-34.829 +X-16.673Y-34.751 +X-16.306Y-34.668 +X-15.580Y-34.491 +X-15.222Y-34.396 +X-14.867Y-34.297 +X-14.172Y-34.086 +X-13.833Y-33.975 +X-13.501Y-33.859 +X-13.176Y-33.740 +X-12.858Y-33.616 +X-12.548Y-33.488 +X-12.248Y-33.357 +X-11.675Y-33.082 +X-11.404Y-32.938 +X-11.144Y-32.791 +X-10.896Y-32.639 +X-10.659Y-32.484 +X-10.435Y-32.326 +X-10.223Y-32.163 +X-10.025Y-31.997 +X-9.840Y-31.827 +X-9.669Y-31.653 +X-9.512Y-31.476 +X-9.370Y-31.295 +X-9.242Y-31.111 +X-9.130Y-30.923 +X-9.033Y-30.732 +X-8.951Y-30.538 +X-8.885Y-30.340 +X-8.835Y-30.139 +X-8.801Y-29.934 +X-8.782Y-29.727 +X-8.780Y-29.517 +X-8.794Y-29.303 +X-8.825Y-29.087 +X-8.871Y-28.867 +X-8.934Y-28.645 +X-9.006Y-28.435 +X-9.093Y-28.223 +X-9.194Y-28.008 +X-9.308Y-27.792 +X-9.435Y-27.573 +X-9.577Y-27.352 +X-9.898Y-26.903 +X-10.078Y-26.676 +X-10.271Y-26.446 +X-10.693Y-25.982 +X-10.922Y-25.748 +X-11.162Y-25.511 +X-11.675Y-25.034 +X-11.948Y-24.793 +X-12.230Y-24.550 +X-12.824Y-24.061 +X-14.114Y-23.068 +X-17.000Y-21.038 +X-17.380Y-20.782 +X-17.762Y-20.525 +X-18.532Y-20.010 +X-20.077Y-18.979 +X-20.462Y-18.721 +X-20.845Y-18.464 +X-21.602Y-17.949 +X-23.072Y-16.925 +X-23.427Y-16.671 +X-23.776Y-16.417 +X-24.454Y-15.911 +X-25.717Y-14.911 +X-26.011Y-14.664 +X-26.295Y-14.418 +X-26.833Y-13.930 +X-27.086Y-13.688 +X-27.328Y-13.447 +X-27.777Y-12.971 +X-28.000Y-12.715 +X-28.209Y-12.461 +X-28.404Y-12.210 +X-28.583Y-11.961 +X-28.746Y-11.713 +X-28.894Y-11.469 +X-29.026Y-11.226 +X-29.142Y-10.986 +X-29.242Y-10.749 +X-29.325Y-10.514 +X-29.391Y-10.281 +X-29.441Y-10.052 +X-29.475Y-9.825 +X-29.491Y-9.601 +X-29.491Y-9.380 +X-29.475Y-9.162 +X-29.441Y-8.947 +X-29.391Y-8.735 +X-29.325Y-8.526 +X-29.242Y-8.320 +X-29.144Y-8.118 +X-29.029Y-7.919 +X-28.899Y-7.723 +X-28.753Y-7.531 +X-28.592Y-7.342 +X-28.416Y-7.156 +X-28.226Y-6.974 +X-28.022Y-6.796 +X-27.804Y-6.621 +X-27.572Y-6.450 +X-27.071Y-6.119 +X-26.802Y-5.959 +X-26.522Y-5.803 +X-25.929Y-5.502 +X-25.617Y-5.358 +X-25.296Y-5.217 +X-24.628Y-4.948 +X-24.283Y-4.819 +X-23.930Y-4.694 +X-23.207Y-4.457 +X-22.838Y-4.344 +X-22.465Y-4.236 +X-21.707Y-4.031 +X-21.325Y-3.935 +X-20.941Y-3.842 +X-20.170Y-3.670 +X-19.786Y-3.590 +X-19.402Y-3.515 +X-18.642Y-3.375 +X-18.266Y-3.312 +X-17.894Y-3.252 +X-17.527Y-3.197 +X-17.164Y-3.145 +X-16.808Y-3.098 +X-16.459Y-3.055 +X-16.116Y-3.015 +X-15.781Y-2.980 +X-15.477Y-2.951 +X-15.179Y-2.925 +X-14.891Y-2.902 +X-14.611Y-2.883 +X-14.340Y-2.867 +X-14.078Y-2.855 +X-13.827Y-2.845 +X-13.586Y-2.839 +X-13.355Y-2.837 +X-13.136Y-2.837 +X-12.929Y-2.841 +X-12.733Y-2.848 +X-12.549Y-2.858 +X-12.378Y-2.871 +X-12.220Y-2.888 +X-12.074Y-2.907 +X-11.942Y-2.929 +X-11.823Y-2.955 +X-11.717Y-2.983 +X-11.626Y-3.014 +X-11.548Y-3.048 +X-11.484Y-3.084 +X-11.435Y-3.124 +X-11.400Y-3.166 +X-11.379Y-3.210 +X-11.372Y-3.258 +X-11.380Y-3.308 +X-11.403Y-3.360 +X-11.440Y-3.415 +X-11.491Y-3.472 +X-11.556Y-3.532 +X-11.636Y-3.594 +X-11.729Y-3.658 +X-11.837Y-3.724 +X-11.959Y-3.793 +X-12.094Y-3.863 +X-12.242Y-3.936 +X-12.404Y-4.010 +X-12.767Y-4.165 +X-12.967Y-4.245 +X-13.179Y-4.327 +X-13.639Y-4.495 +X-14.691Y-4.850 +X-14.979Y-4.942 +X-15.277Y-5.036 +X-15.897Y-5.225 +X-17.230Y-5.617 +X-20.146Y-6.431 +X-20.516Y-6.532 +X-20.887Y-6.634 +X-21.630Y-6.837 +X-23.106Y-7.241 +X-25.895Y-8.028 +X-26.218Y-8.123 +X-26.535Y-8.218 +X-27.145Y-8.403 +X-28.260Y-8.760 +X-28.514Y-8.846 +X-28.759Y-8.930 +X-29.214Y-9.095 +X-29.425Y-9.175 +X-29.624Y-9.253 +X-29.986Y-9.404 +X-30.149Y-9.477 +X-30.298Y-9.548 +X-30.435Y-9.617 +X-30.559Y-9.684 +X-30.669Y-9.749 +X-30.766Y-9.812 +X-30.849Y-9.872 +X-30.918Y-9.931 +X-30.974Y-9.987 +X-31.016Y-10.041 +X-31.043Y-10.092 +X-31.057Y-10.141 +X-31.057Y-10.188 +X-31.043Y-10.232 +X-31.015Y-10.273 +X-30.972Y-10.312 +X-30.916Y-10.349 +X-30.847Y-10.382 +X-30.763Y-10.413 +X-30.666Y-10.441 +X-30.556Y-10.466 +X-30.432Y-10.489 +X-30.295Y-10.508 +X-30.146Y-10.525 +X-29.984Y-10.539 +X-29.809Y-10.549 +X-29.622Y-10.557 +X-29.424Y-10.562 +X-29.214Y-10.563 +X-28.992Y-10.562 +X-28.760Y-10.557 +X-28.517Y-10.549 +X-28.242Y-10.537 +X-27.955Y-10.521 +X-27.657Y-10.501 +X-27.349Y-10.477 +X-27.031Y-10.450 +X-26.703Y-10.418 +X-26.366Y-10.383 +X-26.021Y-10.343 +X-25.668Y-10.300 +X-25.308Y-10.253 +X-24.568Y-10.146 +X-24.191Y-10.086 +X-23.808Y-10.023 +X-23.032Y-9.883 +X-22.640Y-9.807 +X-22.245Y-9.728 +X-21.453Y-9.556 +X-21.057Y-9.463 +X-20.662Y-9.367 +X-19.876Y-9.162 +X-19.487Y-9.054 +X-19.101Y-8.941 +X-18.343Y-8.704 +X-17.972Y-8.579 +X-17.606Y-8.450 +X-16.896Y-8.180 +X-16.552Y-8.040 +X-16.217Y-7.895 +X-15.574Y-7.594 +X-15.268Y-7.438 +X-14.972Y-7.277 +X-14.414Y-6.946 +X-14.153Y-6.774 +X-13.905Y-6.599 +X-13.669Y-6.420 +X-13.447Y-6.237 +X-13.239Y-6.051 +X-13.044Y-5.862 +X-12.864Y-5.669 +X-12.699Y-5.472 +X-12.549Y-5.272 +X-12.414Y-5.069 +X-12.294Y-4.862 +X-12.190Y-4.652 +X-12.102Y-4.439 +X-12.030Y-4.223 +X-11.975Y-4.003 +X-11.935Y-3.781 +X-11.911Y-3.555 +X-11.904Y-3.327 +X-11.914Y-3.096 +X-11.939Y-2.862 +X-11.981Y-2.625 +X-12.038Y-2.386 +X-12.112Y-2.144 +X-12.202Y-1.899 +X-12.300Y-1.669 +X-12.412Y-1.436 +X-12.537Y-1.201 +X-12.675Y-0.965 +X-12.826Y-0.726 +X-12.990Y-0.486 +X-13.355Y0.001 +X-13.555Y0.246 +X-13.768Y0.494 +X-14.226Y0.993 +X-15.266Y2.009 +X-15.550Y2.266 +X-15.842Y2.524 +X-16.451Y3.043 +X-17.752Y4.093 +X-20.572Y6.219 +X-20.935Y6.486 +X-21.298Y6.753 +X-22.024Y7.287 +X-23.456Y8.353 +X-23.807Y8.618 +X-24.154Y8.883 +X-24.835Y9.411 +X-26.125Y10.458 +X-26.431Y10.717 +X-26.728Y10.975 +X-27.297Y11.489 +X-27.568Y11.744 +X-27.830Y11.997 +X-28.322Y12.500 +X-28.552Y12.749 +X-28.771Y12.997 +X-28.978Y13.243 +X-29.174Y13.487 +X-29.357Y13.729 +X-29.528Y13.970 +X-29.686Y14.209 +X-29.831Y14.446 +X-29.974Y14.700 +X-30.101Y14.953 +X-30.212Y15.202 +X-30.307Y15.449 +X-30.385Y15.694 +X-30.447Y15.936 +X-30.492Y16.175 +X-30.521Y16.411 +X-30.532Y16.644 +X-30.527Y16.874 +X-30.505Y17.102 +X-30.466Y17.326 +X-30.411Y17.547 +X-30.338Y17.765 +X-30.249Y17.980 +X-30.144Y18.191 +X-30.022Y18.399 +X-29.884Y18.603 +X-29.730Y18.805 +X-29.561Y19.002 +X-29.376Y19.196 +X-29.176Y19.386 +X-28.962Y19.573 +X-28.733Y19.756 +X-28.490Y19.935 +X-28.233Y20.111 +X-27.964Y20.282 +X-27.681Y20.450 +X-27.386Y20.614 +X-27.080Y20.774 +X-26.434Y21.081 +X-26.096Y21.229 +X-25.748Y21.373 +X-25.025Y21.648 +X-24.652Y21.780 +X-24.272Y21.907 +X-23.493Y22.149 +X-23.095Y22.264 +X-22.693Y22.375 +X-21.877Y22.583 +X-21.466Y22.681 +X-21.053Y22.775 +X-20.223Y22.949 +X-19.809Y23.030 +X-19.396Y23.107 +X-18.575Y23.247 +X-18.169Y23.311 +X-17.767Y23.371 +X-16.977Y23.478 +X-16.591Y23.525 +X-16.211Y23.568 +X-15.838Y23.606 +X-15.473Y23.641 +X-15.116Y23.671 +X-14.769Y23.698 +X-14.430Y23.720 +X-14.102Y23.739 +X-13.791Y23.753 +X-13.490Y23.763 +X-13.200Y23.769 +X-12.922Y23.772 +X-12.657Y23.771 +X-12.404Y23.766 +X-12.164Y23.758 +X-11.937Y23.746 +X-11.724Y23.730 +X-11.525Y23.711 +X-11.340Y23.688 +X-11.171Y23.662 +X-11.016Y23.633 +X-10.876Y23.600 +X-10.752Y23.564 +X-10.643Y23.524 +X-10.549Y23.481 +X-10.472Y23.435 +X-10.411Y23.386 +X-10.365Y23.334 +X-10.336Y23.279 +X-10.323Y23.221 +X-10.326Y23.161 +X-10.344Y23.097 +X-10.379Y23.030 +X-10.430Y22.961 +X-10.497Y22.889 +X-10.579Y22.815 +X-10.676Y22.738 +X-10.789Y22.659 +X-10.917Y22.577 +X-11.060Y22.493 +X-11.217Y22.407 +X-11.389Y22.318 +X-11.773Y22.135 +X-11.985Y22.041 +X-12.210Y21.945 +X-12.697Y21.747 +X-12.958Y21.645 +X-13.230Y21.542 +X-13.806Y21.331 +X-15.066Y20.894 +X-15.401Y20.782 +X-15.742Y20.670 +X-16.442Y20.441 +X-17.896Y19.977 +X-20.868Y19.035 +X-21.208Y18.926 +X-21.545Y18.817 +X-22.208Y18.600 +X-23.472Y18.173 +X-23.772Y18.068 +X-24.066Y17.964 +X-24.629Y17.759 +X-25.650Y17.361 +X-25.881Y17.265 +X-26.101Y17.170 +X-26.509Y16.984 +X-26.696Y16.893 +X-26.871Y16.804 +X-27.184Y16.631 +X-27.322Y16.547 +X-27.447Y16.465 +X-27.559Y16.385 +X-27.658Y16.306 +X-27.743Y16.230 +X-27.815Y16.156 +X-27.873Y16.084 +X-27.917Y16.014 +X-27.947Y15.946 +X-27.963Y15.880 +X-27.965Y15.817 +X-27.953Y15.756 +X-27.927Y15.697 +X-27.887Y15.641 +X-27.832Y15.588 +X-27.764Y15.537 +X-27.681Y15.488 +X-27.585Y15.442 +X-27.475Y15.399 +X-27.351Y15.358 +X-27.214Y15.321 +X-27.063Y15.286 +X-26.899Y15.253 +X-26.723Y15.224 +X-26.533Y15.198 +X-26.332Y15.174 +X-26.118Y15.154 +X-25.892Y15.136 +X-25.654Y15.122 +X-25.405Y15.110 +X-25.145Y15.102 +X-24.875Y15.097 +X-24.594Y15.095 +X-24.304Y15.096 +X-24.004Y15.100 +X-23.694Y15.107 +X-23.377Y15.118 +X-23.051Y15.132 +X-22.717Y15.149 +X-22.376Y15.170 +X-21.998Y15.196 +X-21.613Y15.226 +X-21.221Y15.260 +X-20.822Y15.298 +X-20.418Y15.340 +X-20.010Y15.385 +X-19.180Y15.489 +X-18.761Y15.547 +X-18.339Y15.609 +X-17.492Y15.744 +X-17.067Y15.818 +X-16.644Y15.896 +X-15.800Y16.064 +X-15.382Y16.154 +X-14.967Y16.248 +X-14.150Y16.448 +X-13.749Y16.554 +X-13.354Y16.664 +X-12.583Y16.895 +X-12.209Y17.017 +X-11.844Y17.142 +X-11.140Y17.405 +X-10.803Y17.542 +X-10.477Y17.683 +X-9.858Y17.975 +X-9.566Y18.127 +X-9.287Y18.283 +X-9.021Y18.442 +X-8.768Y18.604 +X-8.529Y18.771 +X-8.305Y18.940 +X-8.094Y19.113 +X-7.899Y19.290 +X-7.718Y19.470 +X-7.553Y19.653 +X-7.403Y19.839 +X-7.270Y20.029 +X-7.152Y20.221 +X-7.050Y20.417 +X-6.965Y20.616 +X-6.895Y20.818 +X-6.843Y21.023 +X-6.806Y21.230 +X-6.787Y21.441 +X-6.783Y21.654 +X-6.796Y21.870 +X-6.826Y22.088 +X-6.872Y22.309 +X-6.933Y22.532 +X-7.011Y22.758 +X-7.104Y22.986 +X-7.213Y23.217 +X-7.338Y23.449 +X-7.477Y23.684 +X-7.631Y23.921 +X-7.982Y24.401 +X-8.165Y24.627 +X-8.360Y24.855 +X-8.782Y25.315 +X-9.751Y26.251 +X-10.017Y26.488 +X-10.291Y26.726 +X-10.863Y27.204 +X-12.089Y28.170 +X-12.409Y28.413 +X-12.734Y28.657 +X-13.395Y29.145 +X-14.749Y30.124 +X-17.457Y32.077 +X-17.785Y32.320 +X-18.108Y32.561 +X-18.740Y33.042 +X-19.930Y33.994 +X-20.210Y34.229 +X-20.481Y34.463 +X-20.997Y34.927 +X-21.240Y35.157 +X-21.474Y35.386 +X-21.910Y35.838 +X-22.111Y36.062 +X-22.302Y36.284 +X-22.480Y36.505 +X-22.646Y36.723 +X-22.800Y36.940 +X-22.941Y37.155 +X-23.069Y37.367 +X-23.183Y37.578 +X-23.285Y37.786 +X-23.373Y37.993 +X-23.447Y38.197 +X-23.507Y38.398 +X-23.553Y38.598 +X-23.585Y38.795 +X-23.602Y38.989 +X-23.606Y39.181 +X-23.593Y39.386 +X-23.563Y39.588 +X-23.516Y39.787 +X-23.452Y39.983 +X-23.372Y40.175 +X-23.274Y40.364 +X-23.160Y40.550 +X-23.029Y40.732 +X-22.882Y40.910 +X-22.719Y41.086 +X-22.539Y41.257 +X-22.344Y41.425 +X-22.134Y41.589 +X-21.908Y41.749 +X-21.668Y41.906 +X-21.413Y42.059 +X-21.144Y42.208 +X-20.862Y42.353 +X-20.566Y42.494 +X-20.257Y42.631 +X-19.937Y42.763 +X-19.604Y42.892 +X-18.906Y43.138 +X-18.542Y43.254 +X-18.168Y43.366 +X-17.785Y43.474 +X-17.394Y43.578 +X-16.995Y43.678 +X-16.589Y43.773 +X-15.758Y43.950 +X-15.335Y44.033 +X-14.908Y44.111 +X-14.476Y44.184 +X-14.042Y44.253 +X-13.606Y44.318 +X-13.168Y44.379 +X-12.289Y44.486 +X-11.851Y44.534 +X-11.414Y44.576 +X-10.978Y44.615 +X-10.546Y44.649 +X-10.116Y44.679 +X-9.691Y44.704 +X-9.271Y44.726 +X-8.856Y44.742 +X-8.447Y44.755 +X-8.044Y44.763 +X-7.650Y44.767 +X-7.263Y44.766 +X-6.885Y44.762 +X-6.516Y44.753 +X-6.157Y44.740 +X-5.808Y44.723 +X-5.471Y44.702 +X-5.145Y44.676 +X-4.830Y44.647 +X-4.529Y44.613 +X-4.240Y44.576 +X-3.965Y44.535 +X-3.703Y44.489 +X-3.456Y44.440 +X-3.227Y44.388 +X-3.013Y44.332 +X-2.813Y44.273 +X-2.629Y44.210 +X-2.459Y44.144 +X-2.305Y44.074 +X-2.167Y44.001 +X-2.044Y43.925 +X-1.938Y43.845 +X-1.847Y43.762 +X-1.773Y43.675 +X-1.715Y43.586 +X-1.673Y43.494 +X-1.647Y43.398 +X-1.638Y43.299 +X-1.645Y43.198 +X-1.667Y43.094 +X-1.706Y42.987 +X-1.761Y42.877 +X-1.832Y42.765 +X-1.918Y42.650 +X-2.019Y42.532 +X-2.135Y42.412 +X-2.267Y42.290 +X-2.412Y42.166 +X-2.572Y42.039 +X-2.934Y41.779 +X-3.134Y41.646 +X-3.347Y41.511 +X-3.810Y41.235 +X-4.059Y41.095 +X-4.319Y40.953 +X-4.869Y40.664 +X-6.076Y40.071 +X-8.785Y38.840 +X-9.139Y38.683 +X-9.494Y38.526 +X-10.205Y38.212 +X-11.612Y37.583 +X-14.215Y36.343 +X-14.490Y36.201 +X-14.756Y36.061 +X-15.263Y35.782 +X-15.503Y35.645 +X-15.734Y35.508 +X-16.165Y35.239 +X-16.365Y35.106 +X-16.553Y34.975 +X-16.730Y34.845 +X-16.895Y34.717 +X-17.049Y34.590 +X-17.190Y34.465 +X-17.318Y34.341 +X-17.434Y34.219 +X-17.537Y34.099 +X-17.626Y33.981 +X-17.702Y33.865 +X-17.764Y33.751 +X-17.813Y33.639 +X-17.847Y33.528 +X-17.868Y33.420 +X-17.875Y33.315 +X-17.867Y33.211 +X-17.845Y33.110 +X-17.809Y33.011 +X-17.759Y32.914 +X-17.695Y32.820 +X-17.616Y32.728 +X-17.523Y32.639 +X-17.417Y32.552 +X-17.296Y32.468 +X-17.162Y32.387 +X-17.014Y32.308 +X-16.852Y32.232 +X-16.677Y32.159 +X-16.489Y32.089 +X-16.288Y32.021 +X-16.075Y31.956 +X-15.849Y31.895 +X-15.611Y31.836 +X-15.361Y31.780 +X-15.099Y31.727 +X-14.827Y31.677 +X-14.543Y31.631 +X-14.250Y31.587 +X-13.946Y31.547 +X-13.632Y31.509 +X-13.309Y31.475 +X-12.977Y31.444 +X-12.637Y31.417 +X-12.289Y31.392 +X-11.933Y31.371 +X-11.570Y31.353 +X-11.201Y31.338 +X-10.825Y31.327 +X-10.444Y31.319 +X-10.058Y31.314 +X-9.667Y31.313 +X-9.273Y31.315 +X-8.874Y31.320 +X-8.473Y31.329 +X-8.069Y31.341 +X-7.630Y31.358 +X-7.188Y31.379 +X-6.746Y31.404 +X-6.303Y31.433 +X-5.861Y31.466 +X-5.421Y31.503 +X-4.982Y31.544 +X-4.546Y31.589 +X-4.113Y31.638 +X-3.684Y31.691 +X-3.260Y31.748 +X-2.841Y31.809 +X-2.428Y31.873 +X-2.021Y31.942 +X-1.231Y32.091 +X-0.848Y32.171 +X-0.475Y32.255 +X-0.110Y32.343 +X0.243Y32.434 +X0.587Y32.530 +X0.919Y32.629 +X1.547Y32.838 +X1.842Y32.948 +X2.124Y33.061 +X2.392Y33.178 +X2.647Y33.299 +X2.887Y33.423 +X3.112Y33.550 +X3.323Y33.681 +X3.518Y33.815 +X3.698Y33.952 +X3.862Y34.092 +X4.010Y34.236 +X4.142Y34.383 +X4.257Y34.533 +X4.356Y34.685 +X4.439Y34.841 +X4.505Y35.000 +X4.554Y35.161 +X4.586Y35.325 +X4.602Y35.492 +X4.602Y35.662 +X4.584Y35.834 +X4.551Y36.008 +X4.501Y36.185 +X4.435Y36.364 +X4.353Y36.546 +X4.255Y36.730 +X4.142Y36.916 +X4.014Y37.104 +X3.871Y37.294 +X3.713Y37.486 +X3.356Y37.876 +X3.157Y38.073 +X2.944Y38.272 +X2.483Y38.675 +X1.425Y39.496 +X1.155Y39.690 +X0.877Y39.885 +X0.300Y40.277 +X-0.925Y41.070 +X-3.540Y42.667 +X-3.871Y42.867 +X-4.202Y43.066 +X-4.858Y43.463 +X-6.136Y44.252 +X-6.444Y44.447 +X-6.748Y44.642 +X-7.337Y45.028 +X-8.430Y45.788 +X-8.682Y45.975 +X-8.926Y46.161 +X-9.383Y46.527 +X-9.596Y46.708 +X-9.799Y46.887 +X-10.170Y47.241 +X-10.337Y47.415 +X-10.493Y47.587 +X-10.636Y47.757 +X-10.766Y47.925 +X-10.884Y48.091 +X-10.988Y48.255 +X-11.078Y48.416 +X-11.155Y48.576 +X-11.218Y48.733 +X-11.267Y48.888 +X-11.301Y49.040 +X-11.322Y49.190 +X-11.328Y49.337 +X-11.319Y49.482 +X-11.296Y49.624 +X-11.259Y49.763 +X-11.207Y49.900 +X-11.140Y50.033 +X-11.059Y50.164 +X-10.963Y50.293 +X-10.853Y50.418 +X-10.729Y50.540 +X-10.591Y50.659 +X-10.439Y50.775 +X-10.276Y50.886 +X-10.100Y50.994 +X-9.912Y51.098 +X-9.710Y51.200 +X-9.496Y51.298 +X-9.270Y51.394 +X-8.783Y51.574 +X-8.522Y51.660 +X-8.250Y51.742 +X-7.968Y51.821 +X-7.675Y51.897 +X-7.372Y51.969 +X-7.059Y52.038 +X-6.407Y52.165 +X-6.068Y52.223 +X-5.722Y52.278 +X-5.368Y52.330 +X-5.006Y52.378 +X-4.639Y52.422 +X-4.265Y52.463 +X-3.886Y52.500 +X-3.501Y52.533 +X-3.112Y52.563 +X-2.719Y52.590 +X-2.322Y52.613 +X-1.923Y52.632 +X-1.521Y52.647 +X-1.116Y52.659 +X-0.711Y52.667 +X-0.304Y52.672 +X0.103Y52.673 +X0.510Y52.670 +X0.916Y52.664 +X1.321Y52.654 +X1.724Y52.640 +X2.125Y52.623 +X2.524Y52.602 +X2.919Y52.577 +X3.310Y52.549 +X3.696Y52.517 +X4.078Y52.481 +X4.455Y52.442 +X4.826Y52.400 +X5.190Y52.354 +X5.548Y52.304 +X5.898Y52.251 +X6.241Y52.194 +X6.576Y52.134 +X6.902Y52.070 +X7.219Y52.003 +X7.526Y51.933 +X7.824Y51.859 +X8.389Y51.701 +X8.656Y51.617 +X8.911Y51.530 +X9.154Y51.439 +X9.386Y51.346 +X9.606Y51.249 +X9.814Y51.149 +X10.009Y51.046 +X10.191Y50.939 +X10.374Y50.821 +X10.541Y50.698 +X10.693Y50.573 +X10.829Y50.444 +X10.948Y50.311 +X11.052Y50.175 +X11.139Y50.036 +X11.209Y49.894 +X11.263Y49.748 +X11.301Y49.600 +X11.323Y49.448 +X11.327Y49.293 +X11.316Y49.136 +X11.288Y48.975 +X11.245Y48.812 +X11.185Y48.646 +X11.109Y48.478 +X11.018Y48.307 +X10.912Y48.133 +X10.790Y47.957 +X10.654Y47.779 +X10.503Y47.598 +X10.159Y47.230 +X9.967Y47.043 +X9.762Y46.854 +X9.314Y46.470 +X8.282Y45.681 +X7.999Y45.480 +X7.707Y45.278 +X7.098Y44.870 +X5.798Y44.041 +X3.014Y42.350 +X2.662Y42.138 +X2.311Y41.925 +X1.617Y41.500 +X0.273Y40.654 +X-0.049Y40.444 +X-0.365Y40.234 +X-0.975Y39.817 +X-2.088Y38.994 +X-2.324Y38.805 +X-2.551Y38.617 +X-2.974Y38.245 +X-3.170Y38.061 +X-3.354Y37.878 +X-3.687Y37.516 +X-3.836Y37.338 +X-3.972Y37.162 +X-4.096Y36.987 +X-4.206Y36.813 +X-4.304Y36.642 +X-4.388Y36.473 +X-4.459Y36.305 +X-4.515Y36.139 +X-4.558Y35.976 +X-4.587Y35.815 +X-4.602Y35.655 +X-4.603Y35.498 +X-4.589Y35.343 +X-4.561Y35.191 +X-4.519Y35.041 +X-4.462Y34.893 +X-4.391Y34.748 +X-4.306Y34.605 +X-4.207Y34.465 +X-4.094Y34.327 +X-3.967Y34.192 +X-3.826Y34.060 +X-3.671Y33.930 +X-3.503Y33.803 +X-3.321Y33.679 +X-3.126Y33.558 +X-2.919Y33.440 +X-2.698Y33.324 +X-2.466Y33.212 +X-2.221Y33.102 +X-1.697Y32.893 +X-1.418Y32.792 +X-1.128Y32.695 +X-0.828Y32.601 +X-0.518Y32.510 +X-0.198Y32.422 +X0.131Y32.338 +X0.814Y32.178 +X1.168Y32.104 +X1.529Y32.032 +X1.897Y31.964 +X2.271Y31.899 +X2.651Y31.838 +X3.037Y31.780 +X3.822Y31.674 +X4.221Y31.626 +X4.622Y31.581 +X5.027Y31.540 +X5.434Y31.502 +X5.842Y31.468 +X6.252Y31.437 +X6.662Y31.410 +X7.072Y31.386 +X7.515Y31.363 +X7.957Y31.345 +X8.398Y31.331 +X8.835Y31.321 +X9.269Y31.315 +X9.699Y31.313 +X10.123Y31.315 +X10.543Y31.321 +X10.956Y31.330 +X11.362Y31.344 +X11.761Y31.362 +X12.152Y31.383 +X12.534Y31.409 +X12.907Y31.438 +X13.269Y31.471 +X13.622Y31.508 +X13.963Y31.549 +X14.293Y31.593 +X14.611Y31.641 +X14.916Y31.693 +X15.209Y31.749 +X15.487Y31.808 +X15.753Y31.870 +X16.003Y31.936 +X16.240Y32.006 +X16.461Y32.079 +X16.667Y32.155 +X16.858Y32.235 +X17.033Y32.318 +X17.191Y32.404 +X17.334Y32.493 +X17.460Y32.586 +X17.570Y32.681 +X17.662Y32.780 +X17.739Y32.881 +X17.798Y32.986 +X17.840Y33.093 +X17.866Y33.203 +X17.875Y33.316 +X17.867Y33.431 +X17.842Y33.549 +X17.801Y33.670 +X17.743Y33.793 +X17.669Y33.918 +X17.579Y34.045 +X17.473Y34.175 +X17.352Y34.307 +X17.215Y34.441 +X17.063Y34.577 +X16.897Y34.715 +X16.522Y34.997 +X16.314Y35.140 +X16.093Y35.285 +X15.614Y35.580 +X15.357Y35.729 +X15.089Y35.879 +X14.522Y36.184 +X13.280Y36.806 +X12.957Y36.960 +X12.628Y37.115 +X11.954Y37.427 +X10.561Y38.054 +X7.746Y39.303 +X7.404Y39.457 +X7.067Y39.611 +X6.409Y39.915 +X5.172Y40.511 +X4.883Y40.657 +X4.603Y40.802 +X4.074Y41.087 +X3.825Y41.227 +X3.588Y41.365 +X3.148Y41.636 +X2.947Y41.769 +X2.760Y41.900 +X2.585Y42.029 +X2.425Y42.156 +X2.278Y42.280 +X2.146Y42.402 +X2.028Y42.522 +X1.926Y42.639 +X1.839Y42.754 +X1.767Y42.866 +X1.711Y42.976 +X1.671Y43.083 +X1.646Y43.187 +X1.638Y43.289 +X1.645Y43.387 +X1.669Y43.483 +X1.709Y43.576 +X1.765Y43.665 +X1.838Y43.752 +X1.926Y43.835 +X2.030Y43.915 +X2.150Y43.992 +X2.286Y44.065 +X2.438Y44.135 +X2.605Y44.202 +X2.787Y44.265 +X2.984Y44.324 +X3.195Y44.380 +X3.421Y44.433 +X3.661Y44.481 +X3.915Y44.526 +X4.182Y44.568 +X4.443Y44.603 +X4.714Y44.635 +X4.996Y44.663 +X5.289Y44.688 +X5.591Y44.710 +X5.902Y44.728 +X6.223Y44.743 +X6.552Y44.754 +X6.890Y44.762 +X7.235Y44.766 +X7.587Y44.767 +X7.946Y44.764 +X8.311Y44.758 +X8.683Y44.748 +X9.059Y44.735 +X9.440Y44.717 +X9.826Y44.697 +X10.216Y44.672 +X10.608Y44.644 +X11.004Y44.613 +X11.401Y44.578 +X11.800Y44.539 +X12.602Y44.450 +X13.003Y44.400 +X13.403Y44.347 +X13.803Y44.289 +X14.201Y44.229 +X14.596Y44.164 +X14.990Y44.096 +X15.766Y43.949 +X16.148Y43.870 +X16.525Y43.787 +X17.263Y43.611 +X17.623Y43.518 +X17.976Y43.421 +X18.661Y43.217 +X18.991Y43.109 +X19.313Y42.999 +X19.626Y42.884 +X19.929Y42.767 +X20.223Y42.645 +X20.506Y42.521 +X21.041Y42.262 +X21.292Y42.127 +X21.531Y41.990 +X21.758Y41.849 +X21.974Y41.704 +X22.176Y41.557 +X22.366Y41.407 +X22.544Y41.253 +X22.708Y41.097 +X22.859Y40.937 +X22.996Y40.775 +X23.119Y40.609 +X23.229Y40.441 +X23.325Y40.269 +X23.407Y40.095 +X23.475Y39.919 +X23.529Y39.739 +X23.571Y39.541 +X23.597Y39.341 +X23.606Y39.137 +X23.599Y38.930 +X23.575Y38.721 +X23.534Y38.508 +X23.477Y38.293 +X23.404Y38.075 +X23.315Y37.855 +X23.211Y37.632 +X23.091Y37.406 +X22.955Y37.178 +X22.805Y36.948 +X22.640Y36.716 +X22.268Y36.244 +X22.061Y36.005 +X21.841Y35.765 +X21.364Y35.278 +X20.274Y34.284 +X19.975Y34.031 +X19.668Y33.778 +X19.029Y33.267 +X17.670Y32.234 +X17.316Y31.974 +X16.959Y31.713 +X16.236Y31.190 +X14.771Y30.140 +X14.405Y29.877 +X14.040Y29.614 +X13.319Y29.089 +X11.923Y28.043 +X11.588Y27.783 +X11.260Y27.524 +X10.626Y27.008 +X10.322Y26.752 +X10.027Y26.497 +X9.467Y25.990 +X9.203Y25.738 +X8.951Y25.488 +X8.482Y24.992 +X8.266Y24.747 +X8.064Y24.503 +X7.700Y24.021 +X7.550Y23.799 +X7.412Y23.578 +X7.288Y23.359 +X7.177Y23.143 +X7.079Y22.928 +X6.995Y22.715 +X6.925Y22.504 +X6.868Y22.295 +X6.826Y22.089 +X6.798Y21.884 +X6.784Y21.682 +X6.785Y21.483 +X6.800Y21.285 +X6.829Y21.091 +X6.873Y20.898 +X6.931Y20.708 +X7.003Y20.521 +X7.090Y20.336 +X7.191Y20.154 +X7.306Y19.975 +X7.434Y19.799 +X7.577Y19.625 +X7.733Y19.454 +X7.903Y19.286 +X8.085Y19.121 +X8.281Y18.959 +X8.489Y18.800 +X8.710Y18.644 +X8.943Y18.491 +X9.188Y18.341 +X9.711Y18.051 +X9.989Y17.910 +X10.278Y17.773 +X10.884Y17.508 +X11.201Y17.381 +X11.527Y17.257 +X12.203Y17.019 +X12.552Y16.905 +X12.908Y16.795 +X13.639Y16.584 +X14.012Y16.484 +X14.390Y16.387 +X15.159Y16.204 +X15.548Y16.118 +X15.940Y16.035 +X16.729Y15.880 +X17.126Y15.808 +X17.522Y15.739 +X18.315Y15.612 +X18.709Y15.554 +X19.102Y15.499 +X19.492Y15.448 +X19.879Y15.401 +X20.263Y15.357 +X20.642Y15.316 +X21.017Y15.279 +X21.387Y15.245 +X21.744Y15.215 +X22.095Y15.189 +X22.440Y15.166 +X22.778Y15.146 +X23.109Y15.129 +X23.431Y15.116 +X23.746Y15.106 +X24.052Y15.099 +X24.349Y15.095 +X24.636Y15.095 +X24.914Y15.097 +X25.181Y15.103 +X25.438Y15.112 +X25.684Y15.123 +X25.919Y15.138 +X26.142Y15.156 +X26.354Y15.177 +X26.553Y15.200 +X26.740Y15.227 +X26.914Y15.256 +X27.076Y15.288 +X27.225Y15.323 +X27.360Y15.361 +X27.482Y15.402 +X27.591Y15.445 +X27.686Y15.491 +X27.767Y15.539 +X27.835Y15.590 +X27.888Y15.643 +X27.928Y15.699 +X27.954Y15.757 +X27.965Y15.818 +X27.963Y15.881 +X27.947Y15.946 +X27.917Y16.014 +X27.873Y16.083 +X27.816Y16.155 +X27.744Y16.229 +X27.660Y16.305 +X27.562Y16.382 +X27.451Y16.462 +X27.327Y16.544 +X27.042Y16.712 +X26.881Y16.799 +X26.707Y16.887 +X26.326Y17.069 +X26.118Y17.162 +X25.900Y17.256 +X25.433Y17.449 +X24.385Y17.849 +X21.934Y18.690 +X21.575Y18.807 +X21.212Y18.925 +X20.474Y19.161 +X18.977Y19.635 +X16.040Y20.572 +X15.691Y20.686 +X15.348Y20.800 +X14.684Y21.024 +X13.458Y21.457 +X13.176Y21.562 +X12.904Y21.666 +X12.396Y21.868 +X12.159Y21.966 +X11.936Y22.063 +X11.528Y22.250 +X11.345Y22.340 +X11.176Y22.429 +X11.022Y22.515 +X10.882Y22.599 +X10.757Y22.681 +X10.648Y22.760 +X10.554Y22.836 +X10.476Y22.910 +X10.413Y22.982 +X10.367Y23.051 +X10.337Y23.117 +X10.323Y23.180 +X10.325Y23.240 +X10.344Y23.298 +X10.379Y23.352 +X10.430Y23.403 +X10.497Y23.452 +X10.580Y23.497 +X10.680Y23.538 +X10.795Y23.577 +X10.926Y23.612 +X11.072Y23.644 +X11.233Y23.673 +X11.410Y23.698 +X11.601Y23.719 +X11.807Y23.737 +X12.027Y23.751 +X12.260Y23.762 +X12.507Y23.769 +X12.767Y23.772 +X13.040Y23.771 +X13.325Y23.767 +X13.601Y23.760 +X13.887Y23.749 +X14.182Y23.734 +X14.486Y23.717 +X14.799Y23.696 +X15.120Y23.671 +X15.448Y23.643 +X15.784Y23.612 +X16.126Y23.577 +X16.475Y23.538 +X16.829Y23.496 +X17.188Y23.450 +X17.552Y23.401 +X17.921Y23.348 +X18.667Y23.232 +X19.045Y23.169 +X19.424Y23.102 +X20.187Y22.957 +X20.569Y22.879 +X20.951Y22.797 +X21.712Y22.623 +X22.090Y22.531 +X22.466Y22.435 +X23.208Y22.232 +X23.574Y22.125 +X23.935Y22.015 +X24.641Y21.784 +X24.985Y21.663 +X25.323Y21.538 +X25.977Y21.279 +X26.292Y21.144 +X26.599Y21.006 +X27.186Y20.719 +X27.465Y20.571 +X27.734Y20.419 +X27.993Y20.264 +X28.241Y20.106 +X28.477Y19.944 +X28.702Y19.779 +X29.116Y19.440 +X29.305Y19.266 +X29.481Y19.088 +X29.644Y18.908 +X29.794Y18.724 +X29.930Y18.538 +X30.053Y18.349 +X30.162Y18.156 +X30.258Y17.961 +X30.339Y17.763 +X30.406Y17.563 +X30.459Y17.359 +X30.498Y17.153 +X30.522Y16.944 +X30.532Y16.733 +X30.528Y16.519 +X30.510Y16.303 +X30.474Y16.066 +X30.421Y15.826 +X30.352Y15.583 +X30.266Y15.337 +X30.164Y15.089 +X30.046Y14.839 +X29.912Y14.586 +X29.762Y14.330 +X29.597Y14.073 +X29.417Y13.813 +X29.014Y13.287 +X28.791Y13.021 +X28.555Y12.753 +X28.044Y12.211 +X27.769Y11.938 +X27.483Y11.663 +X26.879Y11.109 +X25.553Y9.985 +X22.573Y7.693 +X22.183Y7.405 +X21.791Y7.116 +X21.005Y6.537 +X19.442Y5.382 +X19.058Y5.094 +X18.678Y4.806 +X17.931Y4.232 +X16.513Y3.095 +X16.178Y2.813 +X15.852Y2.532 +X15.229Y1.974 +X14.933Y1.697 +X14.649Y1.422 +X14.115Y0.876 +X13.867Y0.606 +X13.632Y0.337 +X13.411Y0.071 +X13.204Y-0.194 +X13.010Y-0.457 +X12.832Y-0.717 +X12.668Y-0.976 +X12.520Y-1.232 +X12.395Y-1.469 +X12.284Y-1.703 +X12.188Y-1.936 +X12.105Y-2.167 +X12.036Y-2.395 +X11.982Y-2.621 +X11.942Y-2.844 +X11.916Y-3.065 +X11.905Y-3.284 +X11.908Y-3.500 +X11.926Y-3.713 +X11.959Y-3.924 +X12.005Y-4.132 +X12.067Y-4.338 +X12.142Y-4.540 +X12.232Y-4.740 +X12.336Y-4.937 +X12.453Y-5.131 +X12.584Y-5.322 +X12.729Y-5.510 +X12.887Y-5.695 +X13.059Y-5.877 +X13.243Y-6.055 +X13.439Y-6.231 +X13.648Y-6.403 +X13.869Y-6.572 +X14.101Y-6.738 +X14.345Y-6.901 +X14.599Y-7.060 +X14.864Y-7.216 +X15.423Y-7.518 +X15.717Y-7.664 +X16.020Y-7.806 +X16.650Y-8.080 +X16.976Y-8.212 +X17.309Y-8.340 +X17.994Y-8.586 +X18.344Y-8.704 +X18.700Y-8.818 +X19.423Y-9.035 +X20.902Y-9.426 +X21.276Y-9.515 +X21.650Y-9.600 +X22.396Y-9.759 +X22.767Y-9.833 +X23.137Y-9.903 +X23.868Y-10.033 +X24.228Y-10.092 +X24.584Y-10.148 +X24.936Y-10.200 +X25.282Y-10.249 +X25.622Y-10.294 +X25.956Y-10.336 +X26.604Y-10.408 +X26.910Y-10.438 +X27.208Y-10.465 +X27.498Y-10.489 +X27.779Y-10.509 +X28.051Y-10.527 +X28.313Y-10.540 +X28.565Y-10.551 +X28.807Y-10.558 +X29.038Y-10.562 +X29.258Y-10.563 +X29.466Y-10.561 +X29.663Y-10.556 +X29.848Y-10.547 +X30.020Y-10.536 +X30.180Y-10.522 +X30.327Y-10.504 +X30.462Y-10.484 +X30.583Y-10.461 +X30.690Y-10.435 +X30.784Y-10.406 +X30.865Y-10.374 +X30.932Y-10.340 +X30.984Y-10.303 +X31.023Y-10.263 +X31.048Y-10.221 +X31.058Y-10.176 +X31.055Y-10.128 +X31.037Y-10.078 +X31.006Y-10.026 +X30.960Y-9.971 +X30.900Y-9.914 +X30.827Y-9.855 +X30.739Y-9.794 +X30.638Y-9.730 +X30.524Y-9.664 +X30.396Y-9.596 +X30.255Y-9.527 +X30.100Y-9.455 +X29.754Y-9.306 +X29.562Y-9.228 +X29.359Y-9.149 +X28.917Y-8.986 +X27.902Y-8.642 +X27.624Y-8.553 +X27.336Y-8.462 +X26.734Y-8.278 +X25.440Y-7.896 +X22.592Y-7.100 +X22.191Y-6.990 +X21.787Y-6.879 +X20.978Y-6.659 +X19.371Y-6.218 +X18.976Y-6.109 +X18.584Y-6.000 +X17.814Y-5.783 +X16.349Y-5.360 +X16.002Y-5.257 +X15.664Y-5.155 +X15.016Y-4.954 +X14.708Y-4.856 +X14.410Y-4.759 +X13.850Y-4.569 +X13.589Y-4.477 +X13.340Y-4.387 +X12.883Y-4.212 +X12.675Y-4.127 +X12.482Y-4.045 +X12.140Y-3.886 +X11.991Y-3.810 +X11.859Y-3.737 +X11.741Y-3.665 +X11.640Y-3.597 +X11.555Y-3.531 +X11.486Y-3.467 +X11.433Y-3.406 +X11.397Y-3.348 +X11.377Y-3.293 +X11.373Y-3.241 +X11.386Y-3.191 +X11.415Y-3.145 +X11.460Y-3.101 +X11.522Y-3.061 +X11.600Y-3.024 +X11.694Y-2.990 +X11.803Y-2.959 +X11.928Y-2.932 +X12.068Y-2.908 +X12.223Y-2.887 +X12.393Y-2.870 +X12.578Y-2.856 +X12.776Y-2.846 +X12.989Y-2.840 +X13.214Y-2.837 +X13.453Y-2.838 +X13.704Y-2.842 +X13.967Y-2.850 +X14.242Y-2.862 +X14.527Y-2.878 +X14.824Y-2.897 +X15.130Y-2.921 +X15.425Y-2.946 +X15.727Y-2.975 +X16.037Y-3.007 +X16.353Y-3.042 +X16.676Y-3.081 +X17.005Y-3.124 +X17.678Y-3.219 +X18.021Y-3.272 +X18.368Y-3.329 +X19.071Y-3.452 +X19.426Y-3.519 +X19.783Y-3.590 +X20.499Y-3.742 +X20.857Y-3.823 +X21.214Y-3.908 +X21.925Y-4.088 +X22.277Y-4.183 +X22.626Y-4.282 +X23.314Y-4.491 +X23.652Y-4.600 +X23.984Y-4.713 +X24.632Y-4.949 +X24.947Y-5.073 +X25.254Y-5.199 +X25.846Y-5.463 +X26.129Y-5.600 +X26.404Y-5.740 +X26.669Y-5.884 +X26.925Y-6.031 +X27.170Y-6.181 +X27.405Y-6.334 +X27.842Y-6.650 +X28.043Y-6.813 +X28.232Y-6.979 +X28.409Y-7.149 +X28.573Y-7.321 +X28.725Y-7.496 +X28.863Y-7.674 +X28.989Y-7.855 +X29.101Y-8.039 +X29.199Y-8.226 +X29.283Y-8.416 +X29.353Y-8.609 +X29.409Y-8.804 +X29.451Y-9.002 +X29.479Y-9.203 +X29.492Y-9.406 +X29.491Y-9.612 +X29.475Y-9.820 +X29.445Y-10.031 +X29.401Y-10.244 +X29.342Y-10.459 +X29.269Y-10.677 +X29.181Y-10.897 +X29.080Y-11.119 +X28.964Y-11.344 +X28.823Y-11.589 +X28.666Y-11.837 +X28.493Y-12.087 +X28.305Y-12.340 +X28.102Y-12.594 +X27.883Y-12.851 +X27.403Y-13.370 +X27.142Y-13.632 +X26.868Y-13.897 +X26.281Y-14.430 +X24.969Y-15.514 +X24.615Y-15.788 +X24.252Y-16.064 +X23.501Y-16.617 +X21.917Y-17.734 +X18.570Y-19.985 +X18.148Y-20.266 +X17.729Y-20.547 +X16.897Y-21.108 +X15.285Y-22.222 +X14.896Y-22.498 +X14.515Y-22.774 +X13.775Y-23.321 +X12.409Y-24.400 +X12.095Y-24.666 +X11.792Y-24.930 +X11.223Y-25.453 +X10.958Y-25.712 +X10.707Y-25.968 +X10.246Y-26.475 +X10.038Y-26.725 +X9.845Y-26.973 +X9.667Y-27.218 +X9.505Y-27.461 +X9.359Y-27.702 +X9.229Y-27.940 +X9.115Y-28.175 +X9.017Y-28.407 +X8.938Y-28.632 +X8.874Y-28.855 +X8.827Y-29.074 +X8.796Y-29.291 +X8.781Y-29.505 +X8.782Y-29.716 +X8.799Y-29.923 +X8.833Y-30.128 +X8.882Y-30.329 +X8.947Y-30.527 +X9.028Y-30.722 +X9.125Y-30.914 +X9.236Y-31.102 +X9.363Y-31.286 +X9.505Y-31.467 +X9.661Y-31.645 +X9.832Y-31.819 +X10.016Y-31.989 +X10.214Y-32.155 +X10.425Y-32.318 +X10.649Y-32.477 +X10.885Y-32.633 +X11.393Y-32.932 +X11.664Y-33.076 +X11.945Y-33.215 +X12.536Y-33.483 +X12.846Y-33.611 +X13.163Y-33.735 +X13.821Y-33.971 +X14.160Y-34.082 +X14.504Y-34.190 +X15.209Y-34.393 +X15.568Y-34.488 +X15.930Y-34.579 +X16.661Y-34.748 +X17.029Y-34.827 +X17.397Y-34.901 +X17.766Y-34.971 +X18.134Y-35.037 +X18.501Y-35.099 +X18.865Y-35.157 +X19.585Y-35.260 +X19.940Y-35.305 +X20.290Y-35.346 +X20.634Y-35.383 +X20.973Y-35.416 +X21.305Y-35.444 +X21.630Y-35.469 +X21.947Y-35.490 +X22.256Y-35.506 +X22.556Y-35.519 +X22.847Y-35.527 +X23.128Y-35.532 +X23.398Y-35.532 +X23.657Y-35.529 +X23.904Y-35.522 +X24.140Y-35.511 +X24.363Y-35.496 +X24.559Y-35.479 +X24.745Y-35.459 +X24.918Y-35.435 +X25.079Y-35.408 +X25.228Y-35.378 +X25.364Y-35.345 +X25.487Y-35.309 +X25.597Y-35.269 +X25.694Y-35.227 +X25.777Y-35.182 +X25.847Y-35.133 +X25.902Y-35.082 +X25.944Y-35.028 +X25.972Y-34.972 +X25.986Y-34.912 +X25.986Y-34.850 +X25.971Y-34.785 +X25.942Y-34.717 +X25.899Y-34.647 +X25.842Y-34.575 +X25.771Y-34.499 +X25.685Y-34.422 +X25.586Y-34.342 +X25.472Y-34.260 +X25.345Y-34.175 +X25.204Y-34.088 +X24.883Y-33.909 +X24.702Y-33.815 +X24.509Y-33.720 +X24.085Y-33.525 +X23.855Y-33.424 +X23.613Y-33.321 +X23.095Y-33.112 +X21.933Y-32.673 +X19.198Y-31.738 +X12.952Y-29.747 +X12.533Y-29.612 +X12.117Y-29.477 +X11.302Y-29.208 +X9.752Y-28.680 +X9.384Y-28.550 +X9.026Y-28.422 +X8.340Y-28.168 +X7.104Y-27.677 +X6.826Y-27.559 +X6.561Y-27.442 +X6.073Y-27.213 +X5.850Y-27.102 +X5.643Y-26.992 +X5.272Y-26.780 +X5.111Y-26.677 +X4.965Y-26.577 +X4.835Y-26.479 +X4.721Y-26.383 +X4.623Y-26.290 +X4.542Y-26.199 +X4.478Y-26.112 +X4.430Y-26.026 +X4.398Y-25.944 +X4.384Y-25.865 +X4.385Y-25.788 +X4.403Y-25.714 +X4.438Y-25.644 +X4.489Y-25.576 +X4.556Y-25.512 +X4.639Y-25.450 +X4.738Y-25.392 +X4.852Y-25.337 +X4.981Y-25.286 +X5.126Y-25.238 +X5.285Y-25.193 +X5.458Y-25.152 +X5.646Y-25.115 +X5.847Y-25.080 +X6.061Y-25.050 +X6.287Y-25.023 +X6.527Y-25.000 +X6.777Y-24.980 +X7.040Y-24.964 +X7.313Y-24.952 +X7.596Y-24.944 +X7.889Y-24.939 +X8.191Y-24.939 +X8.502Y-24.942 +X8.820Y-24.949 +X9.146Y-24.960 +X9.478Y-24.975 +X9.817Y-24.994 +X10.161Y-25.017 +X10.510Y-25.044 +X10.839Y-25.072 +X11.171Y-25.104 +X11.506Y-25.140 +X11.842Y-25.179 +X12.180Y-25.221 +X12.519Y-25.267 +X13.196Y-25.370 +X13.534Y-25.427 +X13.870Y-25.487 +X14.204Y-25.550 +X14.535Y-25.617 +X14.864Y-25.688 +X15.188Y-25.762 +X15.824Y-25.920 +X16.134Y-26.004 +X16.438Y-26.092 +X16.736Y-26.183 +X17.027Y-26.277 +X17.311Y-26.375 +X17.587Y-26.476 +X18.113Y-26.689 +X18.362Y-26.800 +X18.602Y-26.914 +X18.832Y-27.032 +X19.052Y-27.152 +X19.260Y-27.276 +X19.457Y-27.403 +X19.643Y-27.534 +X19.817Y-27.667 +X19.979Y-27.803 +X20.128Y-27.943 +X20.264Y-28.085 +X20.387Y-28.230 +X20.497Y-28.379 +X20.594Y-28.530 +X20.676Y-28.684 +X20.745Y-28.840 +X20.800Y-29.000 +X20.841Y-29.162 +X20.867Y-29.327 +X20.879Y-29.494 +X20.876Y-29.664 +X20.859Y-29.837 +X20.828Y-30.012 +X20.781Y-30.189 +X20.721Y-30.369 +X20.645Y-30.551 +X20.556Y-30.735 +X20.452Y-30.922 +X20.333Y-31.110 +X20.201Y-31.301 +X20.055Y-31.494 +X19.894Y-31.688 +X19.721Y-31.885 +X19.533Y-32.084 +X19.120Y-32.486 +X18.893Y-32.690 +X18.655Y-32.896 +X18.142Y-33.311 +X17.874Y-33.517 +X17.595Y-33.724 +X17.007Y-34.142 +X15.720Y-34.991 +X12.798Y-36.727 +X12.410Y-36.947 +X12.018Y-37.167 +X11.225Y-37.607 +X9.621Y-38.487 +X6.455Y-40.236 +X6.074Y-40.452 +X5.697Y-40.668 +X4.960Y-41.096 +X3.566Y-41.941 +X3.237Y-42.149 +X2.916Y-42.356 +X2.302Y-42.766 +X1.194Y-43.568 +X0.944Y-43.764 +X0.706Y-43.959 +X0.264Y-44.342 +X0.062Y-44.531 +X-0.128Y-44.718 +X-0.469Y-45.085 +X-0.620Y-45.266 +X-0.758Y-45.444 +X-0.882Y-45.620 +X-0.993Y-45.794 +X-1.090Y-45.965 +X-1.173Y-46.134 +X-1.243Y-46.301 +X-1.298Y-46.465 +X-1.342Y-46.640 +X-1.371Y-46.812 +X-1.382Y-46.980 +X-1.378Y-47.146 +X-1.357Y-47.308 +X-1.320Y-47.467 +X-1.267Y-47.622 +X-1.198Y-47.775 +X-1.114Y-47.923 +X-1.014Y-48.068 +X-0.899Y-48.210 +X-0.769Y-48.348 +X-0.625Y-48.482 +X-0.466Y-48.612 +X-0.293Y-48.739 +X-0.107Y-48.862 +X0.093Y-48.981 +X0.305Y-49.097 +X0.530Y-49.208 +X0.766Y-49.315 +X1.014Y-49.419 +X1.273Y-49.518 +X1.822Y-49.705 +X2.110Y-49.792 +X2.407Y-49.875 +X2.713Y-49.954 +X3.026Y-50.029 +X3.346Y-50.100 +X3.672Y-50.166 +X4.004Y-50.229 +X4.341Y-50.287 +X4.683Y-50.340 +X5.028Y-50.390 +X5.376Y-50.435 +X5.727Y-50.476 +X6.080Y-50.513 +X6.433Y-50.545 +X6.787Y-50.573 +X7.141Y-50.597 +X7.494Y-50.617 +X7.845Y-50.632 +X8.194Y-50.643 +X8.540Y-50.650 +X8.882Y-50.652 +X9.220Y-50.651 +X9.553Y-50.645 +X9.881Y-50.635 +X10.202Y-50.620 +X10.516Y-50.602 +X10.823Y-50.579 +X11.122Y-50.552 +X11.412Y-50.521 +X11.693Y-50.486 +X11.964Y-50.447 +X12.225Y-50.404 +X12.475Y-50.356 +X12.714Y-50.305 +X12.941Y-50.250 +X13.155Y-50.191 +X13.357Y-50.128 +X13.546Y-50.061 +X13.721Y-49.990 +X13.882Y-49.916 +X14.020Y-49.843 +X14.144Y-49.767 +X14.256Y-49.688 +X14.355Y-49.605 +X14.441Y-49.520 +X14.513Y-49.431 +X14.571Y-49.340 +X14.615Y-49.245 +X14.645Y-49.148 +X14.662Y-49.048 +X14.664Y-48.944 +X14.651Y-48.838 +X14.625Y-48.730 +X14.584Y-48.618 +X14.529Y-48.504 +X14.459Y-48.387 +X14.376Y-48.268 +X14.278Y-48.146 +X14.166Y-48.022 +X14.040Y-47.896 +X13.900Y-47.767 +X13.747Y-47.635 +X13.399Y-47.366 +X13.206Y-47.228 +X12.999Y-47.088 +X12.548Y-46.803 +X11.504Y-46.209 +X11.215Y-46.057 +X10.916Y-45.902 +X10.287Y-45.589 +X8.923Y-44.948 +X5.870Y-43.614 +X5.468Y-43.444 +X5.063Y-43.274 +X4.246Y-42.931 +X2.603Y-42.244 +X-0.599Y-40.871 +X-1.025Y-40.682 +X-1.444Y-40.493 +X-2.257Y-40.118 +X-3.764Y-39.380 +X-4.113Y-39.199 +X-4.450Y-39.019 +X-5.084Y-38.665 +X-5.380Y-38.490 +X-5.662Y-38.317 +X-6.181Y-37.977 +X-6.417Y-37.810 +X-6.637Y-37.645 +X-6.841Y-37.483 +X-7.028Y-37.322 +X-7.199Y-37.165 +X-7.352Y-37.009 +X-7.488Y-36.856 +X-7.607Y-36.706 +X-7.708Y-36.559 +X-7.791Y-36.414 +X-7.856Y-36.272 +X-7.904Y-36.133 +X-7.934Y-35.997 +X-7.946Y-35.864 +X-7.941Y-35.734 +X-7.918Y-35.608 +X-7.877Y-35.484 +X-7.819Y-35.364 +X-7.744Y-35.247 +X-7.652Y-35.134 +X-7.543Y-35.024 +X-7.418Y-34.918 +X-7.277Y-34.815 +X-7.121Y-34.716 +X-6.949Y-34.620 +X-6.762Y-34.528 +X-6.560Y-34.440 +X-6.345Y-34.356 +X-6.116Y-34.276 +X-5.874Y-34.199 +X-5.620Y-34.127 +X-5.353Y-34.058 +X-5.076Y-33.993 +X-4.787Y-33.933 +X-4.489Y-33.876 +X-4.181Y-33.824 +X-3.864Y-33.775 +X-3.539Y-33.731 +X-3.207Y-33.691 +X-2.867Y-33.655 +X-2.522Y-33.623 +X-2.172Y-33.596 +X-1.817Y-33.572 +X-1.458Y-33.553 +X-1.096Y-33.538 +X-0.732Y-33.528 +X-0.366Y-33.521 +X0.000Y-33.519 +G0Z6.000 +G0X37.560Y12.327Z6.000 +G1Z-1.000 +G1Y0.876 +X49.011 +Y12.327 +X37.560 +G0Z6.000 +G0Y0.876 +G1Z-1.000 +G1Y-10.575 +X49.011 +Y0.876 +X37.560 +G0Z6.000 +G0X49.011Y12.327 +G1Z-1.000 +G1X52.084Y15.011 +G0Z6.000 +G0X49.011Y0.876 +G1Z-1.000 +G1X52.084Y6.213 +Y15.011 +X43.286 +X37.560Y12.327 +G0Z6.000 +G0X49.011Y-10.575 +G1Z-1.000 +G1X52.084Y-2.585 +Y6.213 +X49.011Y0.876 +G0Z6.000 +G0Z20.000 +G0X0.000Y0.000 +M30 diff --git a/gcode.c b/gcode.c index fc6ad6e..8948bea 100644 --- a/gcode.c +++ b/gcode.c @@ -101,7 +101,9 @@ uint8_t gc_execute_line(char *line) float target[N_AXIS]; clear_vector(target); // XYZ(ABC) axes parameters. + #ifdef USE_LINE_NUMBERS int32_t line_number = 0; + #endif gc.arc_radius = 0; clear_vector(gc.arc_offset); // IJK Arc offsets are incremental. Value of zero indicates no change. @@ -227,7 +229,11 @@ uint8_t gc_execute_line(char *line) break; case 'I': case 'J': case 'K': gc.arc_offset[letter-'I'] = to_millimeters(value); break; case 'L': l = trunc(value); break; - case 'N': line_number = trunc(value); break; + case 'N': + #ifdef USE_LINE_NUMBERS + line_number = trunc(value); + #endif + break; case 'P': p = value; break; case 'R': gc.arc_radius = to_millimeters(value); break; case 'S': @@ -331,7 +337,11 @@ uint8_t gc_execute_line(char *line) target[idx] = gc.position[idx]; } } + #ifdef USE_LINE_NUMBERS mc_line(target, -1.0, false, line_number); + #else + mc_line(target, -1.0, false); + #endif } // Retreive G28/30 go-home position data (in machine coordinates) from EEPROM float coord_data[N_AXIS]; @@ -340,7 +350,11 @@ uint8_t gc_execute_line(char *line) } else { if (!settings_read_coord_data(SETTING_INDEX_G30,coord_data)) { return(STATUS_SETTING_READ_FAIL); } } + #ifdef USE_LINE_NUMBERS mc_line(coord_data, -1.0, false, line_number); + #else + mc_line(coord_data, -1.0, false); + #endif memcpy(gc.position, coord_data, sizeof(coord_data)); // gc.position[] = coord_data[]; axis_words = 0; // Axis words used. Lock out from motion modes by clearing flags. break; @@ -411,7 +425,13 @@ uint8_t gc_execute_line(char *line) break; case MOTION_MODE_SEEK: if (!axis_words) { FAIL(STATUS_INVALID_STATEMENT);} - else { mc_line(target, -1.0, false, line_number); } + else { + #ifdef USE_LINE_NUMBERS + mc_line(target, -1.0, false, line_number); + #else + mc_line(target, -1.0, false); + #endif + } break; case MOTION_MODE_LINEAR: // TODO: Inverse time requires F-word with each statement. Need to do a check. Also need @@ -419,7 +439,13 @@ uint8_t gc_execute_line(char *line) // and after an inverse time move and then check for non-zero feed rate each time. This // should be efficient and effective. if (!axis_words) { FAIL(STATUS_INVALID_STATEMENT);} - else { mc_line(target, (gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode, line_number); } + else { + #ifdef USE_LINE_NUMBERS + mc_line(target, (gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode, line_number); + #else + mc_line(target, (gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode); + #endif + } break; case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC: // Check if at least one of the axes of the selected plane has been specified. If in center @@ -441,9 +467,15 @@ uint8_t gc_execute_line(char *line) if (gc.motion_mode == MOTION_MODE_CW_ARC) { isclockwise = true; } // Trace the arc + #ifdef USE_LINE_NUMBERS mc_arc(gc.position, target, gc.arc_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.arc_radius, isclockwise, line_number); + #else + mc_arc(gc.position, target, gc.arc_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.arc_radius, isclockwise); + #endif } break; } diff --git a/limits.c b/limits.c index fbbfb14..636701d 100644 --- a/limits.c +++ b/limits.c @@ -50,9 +50,9 @@ void limits_init() } #ifdef ENABLE_SOFTWARE_DEBOUNCE - MCUSR &= ~(1< Date: Thu, 27 Feb 2014 22:30:24 -0700 Subject: [PATCH 48/73] Probe cycle line numbers ifdef fixes to get it to compile. - Updated some of the ifdefs when disabling line numbers feature. Getting messy with this compile-time option. This will likely get cleaned up later. - This is just a push to get the new probing code to compile. Testing and optimization of the code will soon follow and be pushed next. --- gcode.c | 8 ++++++++ motion_control.c | 16 +++++++++++++++- motion_control.h | 4 ++++ 3 files changed, 27 insertions(+), 1 deletion(-) diff --git a/gcode.c b/gcode.c index aa16d0c..7b31552 100644 --- a/gcode.c +++ b/gcode.c @@ -371,7 +371,11 @@ uint8_t gc_execute_line(char *line) FAIL(STATUS_INVALID_STATEMENT); break; } + #ifdef USE_LINE_NUMBERS if(mc_probe_cycle(target, (gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode, line_number)){ + #else + if(mc_probe_cycle(target, (gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode)){ + #endif FAIL(STATUS_PROBE_ERROR); } axis_words = 0; @@ -381,7 +385,11 @@ uint8_t gc_execute_line(char *line) FAIL(STATUS_INVALID_STATEMENT); break; } + #ifdef USE_LINE_NUMBERS mc_probe_cycle(target, (gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode, line_number); + #else + mc_probe_cycle(target, (gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode); + #endif axis_words = 0; break; case NON_MODAL_SET_HOME_0: case NON_MODAL_SET_HOME_1: diff --git a/motion_control.c b/motion_control.c index cd79d97..00d2423 100644 --- a/motion_control.c +++ b/motion_control.c @@ -269,7 +269,12 @@ void mc_homing_cycle() limits_init(); } + +#ifdef USE_LINE_NUMBERS uint8_t mc_probe_cycle(float *t, float feed_rate, uint8_t invert_feed_rate, int32_t line_number) +#else +uint8_t mc_probe_cycle(float *t, float feed_rate, uint8_t invert_feed_rate) +#endif { protocol_buffer_synchronize(); //finish all queued commands @@ -290,7 +295,12 @@ uint8_t mc_probe_cycle(float *t, float feed_rate, uint8_t invert_feed_rate, int3 // An empty buffer is needed because we need to enable the probe pin along the same move that we're about to execute. sys.state = STATE_CYCLE; + + #ifdef USE_LINE_NUMBERS plan_buffer_line(target, feed_rate, invert_feed_rate, line_number); // Bypass mc_line(). Directly plan homing motion. + #else + plan_buffer_line(target, feed_rate, invert_feed_rate); // Bypass mc_line(). Directly plan homing motion. + #endif st_prep_buffer(); // Prep and fill segment buffer from newly planned block. st_wake_up(); // Initiate motion @@ -341,7 +351,11 @@ uint8_t mc_probe_cycle(float *t, float feed_rate, uint8_t invert_feed_rate, int3 //report_realtime_status(); //debug - plan_buffer_line(target, feed_rate, invert_feed_rate, line_number); // Bypass mc_line(). Directly plan motion. + #ifdef USE_LINE_NUMBERS + plan_buffer_line(target, feed_rate, invert_feed_rate, line_number); // Bypass mc_line(). Directly plan homing motion. + #else + plan_buffer_line(target, feed_rate, invert_feed_rate); // Bypass mc_line(). Directly plan homing motion. + #endif st_prep_buffer(); // Prep and fill segment buffer from newly planned block. st_wake_up(); // Initiate motion diff --git a/motion_control.h b/motion_control.h index f8af924..4956487 100644 --- a/motion_control.h +++ b/motion_control.h @@ -54,7 +54,11 @@ void mc_homing_cycle(); // Perform tool length probe cycle. Requires probe switch. // Returns STATUS_OK in all cases except when the motion is completed without the probe being triggered. // In that case, it returns a STATUS_PROBE_ERROR +#ifdef USE_LINE_NUMBERS uint8_t mc_probe_cycle(float *target, float feed_rate, uint8_t invert_feed_rate, int32_t line_number); +#else +uint8_t mc_probe_cycle(float *target, float feed_rate, uint8_t invert_feed_rate); +#endif // Performs system reset. If in motion state, kills all motion and sets system alarm. void mc_reset(); From 76ab1b6a4282ec09df7b2c1dd534b83e61b1251c Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Fri, 28 Feb 2014 22:03:26 -0700 Subject: [PATCH 49/73] G38.2 probe feature rough draft installed. Working but needs testing. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - G38.2 straight probe now supported. Rough draft. May be tweaked more as testing ramps up. - G38.2 requires at least one axis word. Multiple axis words work too. When commanded, the probe cycle will move at the last ‘F’ feed rate specified in a straight line. - During a probe cycle: If the probe pin goes low (normal high), Grbl will record that immediate position and engage a feed hold. Meaning that the CNC machine will move a little past the probe switch point, so keep federates low to stop sooner. Once stopped, Grbl will issue a move to go back to the recorded probe trigger point. - During a probe cycle: If the probe switch does not engage by the time the machine has traveled to its target coordinates, Grbl will issue an ALARM and the user will be forced to reset Grbl. (Currently G38.3 probe without error isn’t supported, but would be easy to implement later.) - After a successful probe, Grbl will send a feedback message containing the recorded probe coordinates in the machine coordinate system. This is as the g-code standard on probe parameters specifies. - The recorded probe parameters are retained in Grbl memory and can be viewed with the ‘$#’ print parameters command. Upon a power-cycle, not a soft-reset, Grbl will re-zero these values. - Moved ‘$#’ command to require IDLE or ALARM mode, because it accesses EEPROM to fetch the coordinate system offsets. - Updated the Grbl version to v0.9d. - The probe cycle is subject to change upon testing or user-feedback. --- Makefile | 2 +- cpu_map.h | 11 +++++- gcode.c | 44 ++++++++-------------- gcode.h | 11 +++--- limits.c | 6 +-- main.c | 2 + motion_control.c | 98 ++++++++++++------------------------------------ motion_control.h | 6 +-- probe.c | 45 ++++++++++++++++++++++ probe.h | 36 ++++++++++++++++++ protocol.c | 6 ++- report.c | 84 ++++++++++++++--------------------------- report.h | 12 +++--- settings.h | 4 +- stepper.c | 11 +++--- system.c | 17 +++------ system.h | 10 +---- 17 files changed, 195 insertions(+), 210 deletions(-) create mode 100644 probe.c create mode 100644 probe.h diff --git a/Makefile b/Makefile index ea1ea61..ab8c9ec 100644 --- a/Makefile +++ b/Makefile @@ -33,7 +33,7 @@ CLOCK = 16000000 PROGRAMMER ?= -c avrisp2 -P usb OBJECTS = main.o motion_control.o gcode.o spindle_control.o coolant_control.o serial.o \ protocol.o stepper.o eeprom.o settings.o planner.o nuts_bolts.o limits.o \ - print.o report.o system.o + print.o probe.o report.o system.o # FUSES = -U hfuse:w:0xd9:m -U lfuse:w:0x24:m FUSES = -U hfuse:w:0xd2:m -U lfuse:w:0xff:m # update that line with this when programmer is back up: diff --git a/cpu_map.h b/cpu_map.h index 0538bbc..060135a 100644 --- a/cpu_map.h +++ b/cpu_map.h @@ -107,11 +107,18 @@ #define PIN_RESET 0 // Uno Analog Pin 0 #define PIN_FEED_HOLD 1 // Uno Analog Pin 1 #define PIN_CYCLE_START 2 // Uno Analog Pin 2 - #define PIN_PROBE 5 // Uno Analog Pin 5 #define PINOUT_INT PCIE1 // Pin change interrupt enable pin #define PINOUT_INT_vect PCINT1_vect #define PINOUT_PCMSK PCMSK1 // Pin change interrupt register - #define PINOUT_MASK ((1<. +*/ + +#include "system.h" +#include "probe.h" + + +// Probe pin initialization routine. +void probe_init() +{ + PROBE_DDR &= ~(PROBE_MASK); // Configure as input pins + PROBE_PORT |= PROBE_MASK; // Enable internal pull-up resistors. Normal high operation. +} + + +// Monitors probe pin state and records the system position when detected. Called by the +// stepper ISR per ISR tick. +// NOTE: This function must be extremely efficient as to not bog down the stepper ISR. +void probe_state_monitor() +{ + if (sys.probe_state == PROBE_ACTIVE) { + if (!(PROBE_PIN & PROBE_MASK)) { + sys.probe_state = PROBE_OFF; + memcpy(sys.probe_position, sys.position, sizeof(float)*N_AXIS); + sys.execute |= EXEC_FEED_HOLD; + } + } +} diff --git a/probe.h b/probe.h new file mode 100644 index 0000000..e5aef86 --- /dev/null +++ b/probe.h @@ -0,0 +1,36 @@ +/* + probe.h - code pertaining to probing methods + Part of Grbl + + Copyright (c) 2014 Sungeun K. 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 + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#ifndef probe_h +#define probe_h + +// Values that define the probing state machine. +#define PROBE_OFF 0 // No probing. (Must be zero.) +#define PROBE_ACTIVE 1 // Actively watching the input pin. + + +// Probe pin initialization routine. +void probe_init(); + +// Monitors probe pin state and records the system position when detected. Called by the +// stepper ISR per ISR tick. +void probe_state_monitor(); + +#endif diff --git a/protocol.c b/protocol.c index c8406d0..2bf7bcd 100644 --- a/protocol.c +++ b/protocol.c @@ -173,9 +173,11 @@ void protocol_execute_runtime() if (rt_exec & (EXEC_ALARM | EXEC_CRIT_EVENT)) { sys.state = STATE_ALARM; // Set system alarm state - // Critical event. Only hard/soft limit errors currently qualify. + // Critical events. Hard/soft limit events identified by both critical event and alarm exec + // flags. Probe fail is identified by the critical event exec flag only. if (rt_exec & EXEC_CRIT_EVENT) { - report_alarm_message(ALARM_LIMIT_ERROR); + if (rt_exec & EXEC_ALARM) { report_alarm_message(ALARM_LIMIT_ERROR); } + else { report_alarm_message(ALARM_PROBE_FAIL); } report_feedback_message(MESSAGE_CRITICAL_EVENT); bit_false(sys.execute,EXEC_RESET); // Disable any existing reset do { diff --git a/report.c b/report.c index bcecfc8..e766ee5 100644 --- a/report.c +++ b/report.c @@ -79,8 +79,6 @@ void report_status_message(uint8_t status_code) printPgmString(PSTR("Homing not enabled")); break; case STATUS_OVERFLOW: printPgmString(PSTR("Line overflow")); break; - case STATUS_PROBE_ERROR: - printPgmString(PSTR("Probe error")); break; } printPgmString(PSTR("\r\n")); } @@ -95,8 +93,10 @@ void report_alarm_message(int8_t alarm_code) printPgmString(PSTR("Hard/soft limit")); break; case ALARM_ABORT_CYCLE: printPgmString(PSTR("Abort during cycle")); break; + case ALARM_PROBE_FAIL: + printPgmString(PSTR("Probe fail")); break; } - printPgmString(PSTR(". MPos?\r\n")); + printPgmString(PSTR("\r\n")); delay_ms(500); // Force delay to ensure message clears serial write buffer. } @@ -190,8 +190,28 @@ void report_grbl_settings() { } -// Prints gcode coordinate offset parameters -void report_gcode_parameters() +// Prints current probe parameters. Upon a probe command, these parameters are updated upon a +// successful probe or upon a failed probe with the G38.3 without errors command (if supported). +// These values are retained until Grbl is power-cycled, whereby they will be re-zeroed. +void report_probe_parameters() +{ + uint8_t i; + float print_position[N_AXIS]; + + // Report in terms of machine position. + printPgmString(PSTR("[Probe:")); + for (i=0; i< N_AXIS; i++) { + print_position[i] = sys.probe_position[i]/settings.steps_per_mm[i]; + if (bit_istrue(settings.flags,BITFLAG_REPORT_INCHES)) { print_position[i] *= INCH_PER_MM; } + printFloat(print_position[i]); + if (i < (N_AXIS-1)) { printPgmString(PSTR(",")); } + } + printPgmString(PSTR("]\r\n")); +} + + +// Prints Grbl NGC parameters (coordinate offsets, probing) +void report_ngc_parameters() { float coord_data[N_AXIS]; uint8_t coord_select, i; @@ -226,6 +246,7 @@ void report_gcode_parameters() if (i < (N_AXIS-1)) { printPgmString(PSTR(",")); } else { printPgmString(PSTR("]\r\n")); } } + report_probe_parameters(); // Print probe parameters. Not persistent in memory. } @@ -353,7 +374,7 @@ void report_realtime_status() if (i < (N_AXIS-1)) { printPgmString(PSTR(",")); } } -#ifdef USE_LINE_NUMBERS + #ifdef USE_LINE_NUMBERS // Report current line number printPgmString(PSTR(",Ln:")); int32_t ln=0; @@ -362,56 +383,7 @@ void report_realtime_status() ln = pb->line_number; } printInteger(ln); -#endif - - printPgmString(PSTR(">\r\n")); -} - -// Prints real-time data. This function grabs a real-time snapshot of the stepper subprogram - // and the actual location of the CNC machine. Users may change the following function to their - // specific needs. It is kept separate from the "normal" report_realtime_status() to allow customization. -void report_realtime_status_probe() -{ - // **Under construction** Bare-bones status report. Provides real-time machine position relative to - // the system power on location (0,0,0) and work coordinate position (G54 and G92 applied). - uint8_t i; - int32_t current_position[N_AXIS]; // Copy current state of the system position variable - memcpy(current_position,sys.position,sizeof(sys.position)); - float print_position[N_AXIS]; - - printPgmString(PSTR("line_number; - } - printInteger(ln); -#endif + #endif printPgmString(PSTR(">\r\n")); } diff --git a/report.h b/report.h index f05bc42..91d7797 100644 --- a/report.h +++ b/report.h @@ -36,11 +36,11 @@ #define STATUS_ALARM_LOCK 12 #define STATUS_SOFT_LIMIT_ERROR 13 #define STATUS_OVERFLOW 14 -#define STATUS_PROBE_ERROR 15 // Define Grbl alarm codes. Less than zero to distinguish alarm error from status error. #define ALARM_LIMIT_ERROR -1 #define ALARM_ABORT_CYCLE -2 +#define ALARM_PROBE_FAIL -3 // Define Grbl feedback message codes. #define MESSAGE_CRITICAL_EVENT 1 @@ -70,13 +70,11 @@ void report_grbl_settings(); // Prints realtime status report void report_realtime_status(); -// Prints realtime position status report at the end of a probe cycle -// This is in leiu of saving the probe position to internal variables like an -// EMC machine -void report_realtime_status_probe(); +// Prints recorded probe position +void report_probe_parameters(); -// Prints Grbl persistent coordinate parameters -void report_gcode_parameters(); +// Prints Grbl NGC parameters (coordinate offsets, probe) +void report_ngc_parameters(); // Prints current g-code parser mode state void report_gcode_modes(); diff --git a/settings.h b/settings.h index 3551ec2..62dfcfc 100644 --- a/settings.h +++ b/settings.h @@ -25,8 +25,8 @@ #include "system.h" -#define GRBL_VERSION "0.9c" -#define GRBL_VERSION_BUILD "20140215" +#define GRBL_VERSION "0.9d" +#define GRBL_VERSION_BUILD "20140228" // Version of the EEPROM data. Will be used to migrate existing data from older versions of Grbl // when firmware is upgraded. Always stored in byte 0 of eeprom diff --git a/stepper.c b/stepper.c index 370d798..58ec9c1 100644 --- a/stepper.c +++ b/stepper.c @@ -24,6 +24,7 @@ #include "stepper.h" #include "settings.h" #include "planner.h" +#include "probe.h" // Some useful constants. @@ -282,12 +283,6 @@ ISR(TIMER1_COMPA_vect) { // SPINDLE_ENABLE_PORT ^= 1< Date: Fri, 7 Mar 2014 19:33:57 +0100 Subject: [PATCH 50/73] Probing command gets stuck in hold if several g38.2 are submitted Ex. G0 X0 Y0 Z0 G38.2 Z-10 F100 G10 L20 P0 Z0 G0 Z2 G38.2 Z-1 F50 G10 L20 P0 Z0 G0 Z2 G0 X0 Y0 G38.2 Z-1 F100 G0 Z2 --- motion_control.c | 1 + 1 file changed, 1 insertion(+) diff --git a/motion_control.c b/motion_control.c index 3b2080c..6fda0c2 100644 --- a/motion_control.c +++ b/motion_control.c @@ -279,6 +279,7 @@ void mc_probe_cycle(float *target, float feed_rate, uint8_t invert_feed_rate, in void mc_probe_cycle(float *target, float feed_rate, uint8_t invert_feed_rate) #endif { + if (sys.state != STATE_CYCLE) protocol_auto_cycle_start(); protocol_buffer_synchronize(); // Finish all queued commands if (sys.abort) { return; } // Return if system reset has been issued. From d3bf28f025251e4e721565f1afd5e76af647c52c Mon Sep 17 00:00:00 2001 From: henols Date: Fri, 7 Mar 2014 19:33:57 +0100 Subject: [PATCH 51/73] Probing command gets stuck in hold if several g38.2 are submitted Ex. G0 X0 Y0 Z0 G38.2 Z-10 F100 G10 L20 P0 Z0 G0 Z2 G38.2 Z-1 F50 G10 L20 P0 Z0 G0 Z2 G0 X0 Y0 G38.2 Z-1 F100 G0 Z2 --- motion_control.c | 1 + 1 file changed, 1 insertion(+) diff --git a/motion_control.c b/motion_control.c index 3b2080c..6fda0c2 100644 --- a/motion_control.c +++ b/motion_control.c @@ -279,6 +279,7 @@ void mc_probe_cycle(float *target, float feed_rate, uint8_t invert_feed_rate, in void mc_probe_cycle(float *target, float feed_rate, uint8_t invert_feed_rate) #endif { + if (sys.state != STATE_CYCLE) protocol_auto_cycle_start(); protocol_buffer_synchronize(); // Finish all queued commands if (sys.abort) { return; } // Return if system reset has been issued. From 60dd609b7956037369d5591bee33b778a6614efd Mon Sep 17 00:00:00 2001 From: Rob Brown Date: Tue, 11 Mar 2014 09:23:39 +0800 Subject: [PATCH 52/73] Added Probing to Mega2560 and fixed Shapeoko2 compile error --- cpu_map.h | 9 ++++++++- defaults.h | 3 ++- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/cpu_map.h b/cpu_map.h index 060135a..797f6bf 100644 --- a/cpu_map.h +++ b/cpu_map.h @@ -223,10 +223,17 @@ #define PINOUT_PCMSK PCMSK2 // Pin change interrupt register #define PINOUT_MASK ((1< Date: Thu, 13 Mar 2014 14:32:21 -0600 Subject: [PATCH 53/73] Comment corrections and function call update. --- defaults.h | 30 +++++++++++++++--------------- gcode.c | 2 +- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/defaults.h b/defaults.h index 415a44c..9699eba 100644 --- a/defaults.h +++ b/defaults.h @@ -35,9 +35,9 @@ #define DEFAULT_X_MAX_RATE 500.0 // mm/min #define DEFAULT_Y_MAX_RATE 500.0 // mm/min #define DEFAULT_Z_MAX_RATE 500.0 // mm/min - #define DEFAULT_X_ACCELERATION (10.0*60*60) // 10 mm/min^2 - #define DEFAULT_Y_ACCELERATION (10.0*60*60) // 10 mm/min^2 - #define DEFAULT_Z_ACCELERATION (10.0*60*60) // 10 mm/min^2 + #define DEFAULT_X_ACCELERATION (10.0*60*60) // 10*60*60 mm/min^2 = 10 mm/sec^2 + #define DEFAULT_Y_ACCELERATION (10.0*60*60) // 10*60*60 mm/min^2 = 10 mm/sec^2 + #define DEFAULT_Z_ACCELERATION (10.0*60*60) // 10*60*60 mm/min^2 = 10 mm/sec^2 #define DEFAULT_X_MAX_TRAVEL 200.0 // mm #define DEFAULT_Y_MAX_TRAVEL 200.0 // mm #define DEFAULT_Z_MAX_TRAVEL 200.0 // mm @@ -75,9 +75,9 @@ #define DEFAULT_X_MAX_RATE 635.0 // mm/min (25 ipm) #define DEFAULT_Y_MAX_RATE 635.0 // mm/min #define DEFAULT_Z_MAX_RATE 635.0 // mm/min - #define DEFAULT_X_ACCELERATION (50.0*60*60) // 10 mm/min^2 - #define DEFAULT_Y_ACCELERATION (50.0*60*60) // 10 mm/min^2 - #define DEFAULT_Z_ACCELERATION (50.0*60*60) // 10 mm/min^2 + #define DEFAULT_X_ACCELERATION (50.0*60*60) // 50*60*60 mm/min^2 = 50 mm/sec^2 + #define DEFAULT_Y_ACCELERATION (50.0*60*60) // 50*60*60 mm/min^2 = 50 mm/sec^2 + #define DEFAULT_Z_ACCELERATION (50.0*60*60) // 50*60*60 mm/min^2 = 50 mm/sec^2 #define DEFAULT_X_MAX_TRAVEL 225.0 // mm #define DEFAULT_Y_MAX_TRAVEL 125.0 // mm #define DEFAULT_Z_MAX_TRAVEL 170.0 // mm @@ -118,9 +118,9 @@ #define DEFAULT_X_MAX_RATE 1000.0 // mm/min #define DEFAULT_Y_MAX_RATE 1000.0 // mm/min #define DEFAULT_Z_MAX_RATE 1000.0 // mm/min - #define DEFAULT_X_ACCELERATION (15.0*60*60) // 10 mm/min^2 - #define DEFAULT_Y_ACCELERATION (15.0*60*60) // 10 mm/min^2 - #define DEFAULT_Z_ACCELERATION (15.0*60*60) // 10 mm/min^2 + #define DEFAULT_X_ACCELERATION (15.0*60*60) // 15*60*60 mm/min^2 = 15 mm/sec^2 + #define DEFAULT_Y_ACCELERATION (15.0*60*60) // 15*60*60 mm/min^2 = 15 mm/sec^2 + #define DEFAULT_Z_ACCELERATION (15.0*60*60) // 15*60*60 mm/min^2 = 15 mm/sec^2 #define DEFAULT_X_MAX_TRAVEL 200.0 // mm #define DEFAULT_Y_MAX_TRAVEL 200.0 // mm #define DEFAULT_Z_MAX_TRAVEL 200.0 // mm @@ -161,9 +161,9 @@ #define DEFAULT_X_MAX_RATE 800.0 // mm/min #define DEFAULT_Y_MAX_RATE 800.0 // mm/min #define DEFAULT_Z_MAX_RATE 800.0 // mm/min - #define DEFAULT_X_ACCELERATION (15.0*60*60) // 10 mm/min^2 - #define DEFAULT_Y_ACCELERATION (15.0*60*60) // 10 mm/min^2 - #define DEFAULT_Z_ACCELERATION (15.0*60*60) // 10 mm/min^2 + #define DEFAULT_X_ACCELERATION (15.0*60*60) // 15*60*60 mm/min^2 = 15 mm/sec^2 + #define DEFAULT_Y_ACCELERATION (15.0*60*60) // 15*60*60 mm/min^2 = 15 mm/sec^2 + #define DEFAULT_Z_ACCELERATION (15.0*60*60) // 15*60*60 mm/min^2 = 15 mm/sec^2 #define DEFAULT_X_MAX_TRAVEL 200.0 // mm #define DEFAULT_Y_MAX_TRAVEL 200.0 // mm #define DEFAULT_Z_MAX_TRAVEL 200.0 // mm @@ -202,9 +202,9 @@ #define DEFAULT_X_MAX_RATE 6000.0 // mm/min #define DEFAULT_Y_MAX_RATE 6000.0 // mm/min #define DEFAULT_Z_MAX_RATE 6000.0 // mm/min - #define DEFAULT_X_ACCELERATION (600.0*60*60) // 10 mm/min^2 - #define DEFAULT_Y_ACCELERATION (600.0*60*60) // 10 mm/min^2 - #define DEFAULT_Z_ACCELERATION (600.0*60*60) // 10 mm/min^2 + #define DEFAULT_X_ACCELERATION (600.0*60*60) // 600*60*60 mm/min^2 = 600 mm/sec^2 + #define DEFAULT_Y_ACCELERATION (600.0*60*60) // 600*60*60 mm/min^2 = 600 mm/sec^2 + #define DEFAULT_Z_ACCELERATION (600.0*60*60) // 600*60*60 mm/min^2 = 600 mm/sec^2 #define DEFAULT_X_MAX_TRAVEL 190.0 // mm #define DEFAULT_Y_MAX_TRAVEL 180.0 // mm #define DEFAULT_Z_MAX_TRAVEL 150.0 // mm diff --git a/gcode.c b/gcode.c index 93c79ea..0620f6b 100644 --- a/gcode.c +++ b/gcode.c @@ -64,7 +64,7 @@ void gc_init() // Sets g-code parser position in mm. Input in steps. Called by the system abort and hard // limit pull-off routines. -void gc_sync_position(int32_t x, int32_t y, int32_t z) +void gc_sync_position() { uint8_t i; for (i=0; i Date: Sun, 25 May 2014 16:05:28 -0600 Subject: [PATCH 54/73] Major g-code parser overhaul. 100%* compliant. Other related updates. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Completely overhauled the g-code parser. It’s now 100%* compliant. (* may have some bugs). Being compliant, here are some of the major differences. - SMALLER and JUST AS FAST! A number of optimizations were found that sped things up and allowed for the more thorough error-checking to be installed without a speed hit. Trimmed a lot of ‘fat’ in the parser and still was able to make it significantly smaller than it was. - No default feed rate setting! Removed completely! This doesn’t exist in the g-code standard. So, it now errors out whenever it’s undefined for motions that require it (G1/2/3/38.2). - Any g-code parser error expunges the ENTIRE block. This means all information is lost and not passed on to the running state. Before some of the states would remain, which could have led to some problems. - If the g-code block passes all of the error-checks, the g-code state is updated and all motions are executed according to the order of execution. - Changes in spindle speed, when already running, will update the output pin accordingly. This fixes a bug, where it wouldn’t update the speed. - Update g-code parser error reporting. Errors now return detailed information of what exact went wrong. The most common errors return a short text description. For less common errors, the parser reports ‘Invalid gcode ID:20’, where 20 is a error ID. A list of error code IDs and their descriptions will be documented for user reference elsewhere to save flash space. - Other notable changes: - Added a print integer routine for uint8 variables. This saved significant flash space by switching from a heavier universal print integer routine. - Saved some flash space with our own short hypotenuse calculation - Some arc computation flash and memory optimizations. --- config.h | 2 +- coolant_control.c | 3 + coolant_control.h | 5 - defaults.h | 17 +- gcode.c | 1307 +++++++++++++++++++++++++++------------------ gcode.h | 169 ++++-- motion_control.c | 45 +- motion_control.h | 8 +- nuts_bolts.c | 8 +- nuts_bolts.h | 6 +- print.c | 36 +- print.h | 2 + probe.c | 8 +- probe.h | 3 + protocol.c | 45 +- report.c | 124 ++--- report.h | 44 +- settings.c | 40 +- settings.h | 7 +- spindle_control.c | 3 + spindle_control.h | 5 - system.c | 16 +- 22 files changed, 1162 insertions(+), 741 deletions(-) diff --git a/config.h b/config.h index 55114bf..ccba270 100644 --- a/config.h +++ b/config.h @@ -29,7 +29,7 @@ #define config_h // Default settings. Used when resetting EEPROM. Change to desired name in defaults.h -#define DEFAULTS_SHERLINE_5400 +#define DEFAULTS_GENERIC // Serial baud rate #define BAUD_RATE 115200 diff --git a/coolant_control.c b/coolant_control.c index a3f2554..9d66c50 100644 --- a/coolant_control.c +++ b/coolant_control.c @@ -21,6 +21,7 @@ #include "system.h" #include "coolant_control.h" #include "protocol.h" +#include "gcode.h" void coolant_init() @@ -44,6 +45,8 @@ void coolant_stop() void coolant_run(uint8_t mode) { + if (sys.state != STATE_CHECK_MODE) { return; } + protocol_buffer_synchronize(); // Ensure coolant turns on when specified in program. if (mode == COOLANT_FLOOD_ENABLE) { COOLANT_FLOOD_PORT |= (1 << COOLANT_FLOOD_BIT); diff --git a/coolant_control.h b/coolant_control.h index c19a673..83ce30b 100644 --- a/coolant_control.h +++ b/coolant_control.h @@ -22,11 +22,6 @@ #define coolant_control_h -#define COOLANT_MIST_ENABLE 2 -#define COOLANT_FLOOD_ENABLE 1 -#define COOLANT_DISABLE 0 // Must be zero. - - void coolant_init(); void coolant_stop(); void coolant_run(uint8_t mode); diff --git a/defaults.h b/defaults.h index 9699eba..f2726a7 100644 --- a/defaults.h +++ b/defaults.h @@ -42,7 +42,6 @@ #define DEFAULT_Y_MAX_TRAVEL 200.0 // mm #define DEFAULT_Z_MAX_TRAVEL 200.0 // mm #define DEFAULT_STEP_PULSE_MICROSECONDS 10 - #define DEFAULT_FEEDRATE 250.0 // mm/min #define DEFAULT_STEPPING_INVERT_MASK 0 #define DEFAULT_DIRECTION_INVERT_MASK ((1<. */ -/* This code is inspired by the Arduino GCode Interpreter by Mike Ellery and the NIST RS274/NGC Interpreter - by Kramer, Proctor and Messina. */ - #include "system.h" #include "settings.h" #include "protocol.h" @@ -29,34 +26,28 @@ #include "motion_control.h" #include "spindle_control.h" #include "coolant_control.h" +#include "probe.h" #include "report.h" +#define MAX_LINE_NUMBER 99999 + +#define AXIS_COMMAND_NONE 0 +#define AXIS_COMMAND_NON_MODAL 1 +#define AXIS_COMMAND_MOTION_MODE 2 + // Declare gc extern struct -parser_state_t gc; +parser_state_t gc_state; +parser_block_t gc_block; -#define FAIL(status) gc.status_code = status; - -static uint8_t next_statement(char *letter, float *float_ptr, char *line, uint8_t *char_counter); -static void gc_convert_arc_radius_mode(float *target) __attribute__((noinline)); - - -static void select_plane(uint8_t axis_0, uint8_t axis_1, uint8_t axis_2) -{ - gc.plane_axis_0 = axis_0; - gc.plane_axis_1 = axis_1; - gc.plane_axis_2 = axis_2; -} +#define FAIL(status) return(status); void gc_init() { - memset(&gc, 0, sizeof(gc)); - gc.feed_rate = settings.default_feed_rate; - select_plane(X_AXIS, Y_AXIS, Z_AXIS); - gc.absolute_mode = true; + memset(&gc_state, 0, sizeof(gc_state)); // Load default G54 coordinate system. - if (!(settings_read_coord_data(gc.coord_select,gc.coord_system))) { + if (!(settings_read_coord_data(gc_state.modal.coord_select,gc_state.coord_system))) { report_status_message(STATUS_SETTING_READ_FAIL); } } @@ -68,16 +59,19 @@ void gc_sync_position() { uint8_t i; for (i=0; i 255, variable type must be changed to uint16_t. - float inverse_feed_rate = -1; // negative inverse_feed_rate means no inverse_feed_rate specified - uint8_t absolute_override = false; // true(1) = absolute motion for this block only {G53} - uint8_t non_modal_action = NON_MODAL_NONE; // Tracks the actions of modal group 0 (non-modal) - - float target[N_AXIS]; - clear_vector(target); // XYZ(ABC) axes parameters. + while (line[char_counter] != 0) { // Loop until no more g-code words in line. + + // Import the next g-code word, expecting a letter followed by a value. Otherwise, error out. + letter = line[char_counter]; + if((letter < 'A') || (letter > 'Z')) { FAIL(STATUS_EXPECTED_COMMAND_LETTER); } // [Expected word letter] + char_counter++; + if (!read_float(line, &char_counter, &value)) { FAIL(STATUS_BAD_NUMBER_FORMAT); } // [Expected word value] - #ifdef USE_LINE_NUMBERS - int32_t line_number = 0; - #endif - gc.arc_radius = 0; - clear_vector(gc.arc_offset); // IJK Arc offsets are incremental. Value of zero indicates no change. - - gc.status_code = STATUS_OK; - - /* Pass 1: Commands and set all modes. Check for modal group violations. - NOTE: Modal group numbers are defined in Table 4 of NIST RS274-NGC v3, pg.20 */ - uint8_t group_number = MODAL_GROUP_NONE; - while(next_statement(&letter, &value, line, &char_counter)) { + // Convert values to smaller uint8 significand and mantissa values for parsing this word. + // NOTE: Mantissa is multiplied by 1000 to catch non-integer command values. int_value = trunc(value); + mantissa = trunc(1000*(value - int_value)); // Compute mantissa for Gxx.x commands + + // Check if the g-code word is supported or errors due to modal group violations or has + // been repeated in the g-code block. If ok, update the command or record its value. switch(letter) { + + /* 'G' and 'M' Command Words: Parse commands and check for modal group violations. + NOTE: Modal group numbers are defined in Table 4 of NIST RS274-NGC v3, pg.20 */ + case 'G': - // Set modal group values + // Determine 'G' command and its modal group switch(int_value) { - case 4: case 10: case 28: case 30: case 53: case 92: group_number = MODAL_GROUP_0; break; - case 0: case 1: case 2: case 3: case 38: case 80: group_number = MODAL_GROUP_1; break; - case 17: case 18: case 19: group_number = MODAL_GROUP_2; break; - case 90: case 91: group_number = MODAL_GROUP_3; break; - case 93: case 94: group_number = MODAL_GROUP_5; break; - case 20: case 21: group_number = MODAL_GROUP_6; break; - case 54: case 55: case 56: case 57: case 58: case 59: group_number = MODAL_GROUP_12; break; - } - // Set 'G' commands - switch(int_value) { - case 0: gc.motion_mode = MOTION_MODE_SEEK; break; - case 1: gc.motion_mode = MOTION_MODE_LINEAR; break; - case 2: gc.motion_mode = MOTION_MODE_CW_ARC; break; - case 3: gc.motion_mode = MOTION_MODE_CCW_ARC; break; - case 4: non_modal_action = NON_MODAL_DWELL; break; - case 10: non_modal_action = NON_MODAL_SET_COORDINATE_DATA; break; - case 17: select_plane(X_AXIS, Y_AXIS, Z_AXIS); break; - case 18: select_plane(Z_AXIS, X_AXIS, Y_AXIS); break; - case 19: select_plane(Y_AXIS, Z_AXIS, X_AXIS); break; - case 20: gc.inches_mode = true; break; - case 21: gc.inches_mode = false; break; - case 28: case 30: - int_value = trunc(10*value); // Multiply by 10 to pick up Gxx.1 + case 10: case 28: case 30: case 92: + // Check for G10/28/30/92 being called with G0/1/2/3/38 on same block. + if (mantissa == 0) { // Ignore G28.1, G30.1, and G92.1 + if (axis_explicit) { FAIL(STATUS_GCODE_AXIS_COMMAND_CONFLICT); } // [Axis word/command conflict] + axis_explicit = AXIS_COMMAND_NON_MODAL; + } + // No break. Continues to next line. + case 4: case 53: + word_bit = MODAL_GROUP_G0; switch(int_value) { - case 280: non_modal_action = NON_MODAL_GO_HOME_0; break; - case 281: non_modal_action = NON_MODAL_SET_HOME_0; break; - case 300: non_modal_action = NON_MODAL_GO_HOME_1; break; - case 301: non_modal_action = NON_MODAL_SET_HOME_1; break; - default: FAIL(STATUS_UNSUPPORTED_STATEMENT); + case 4: gc_block.non_modal_command = NON_MODAL_DWELL; break; // G4 + case 10: gc_block.non_modal_command = NON_MODAL_SET_COORDINATE_DATA; break; // G10 + case 28: + switch(mantissa) { + case 0: gc_block.non_modal_command = NON_MODAL_GO_HOME_0; break; // G28 + case 100: gc_block.non_modal_command = NON_MODAL_SET_HOME_0; break; // G28.1 + default: FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); // [Unsupported G28.x command] + } + mantissa = 0; // Set to zero to indicate valid non-integer G command. + break; + case 30: + switch(mantissa) { + case 0: gc_block.non_modal_command = NON_MODAL_GO_HOME_1; break; // G30 + case 100: gc_block.non_modal_command = NON_MODAL_SET_HOME_1; break; // G30.1 + default: FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); // [Unsupported G30.x command] + } + mantissa = 0; // Set to zero to indicate valid non-integer G command. + break; + case 53: gc_block.non_modal_command = NON_MODAL_ABSOLUTE_OVERRIDE; break; // G53 + case 92: + switch(mantissa) { + case 0: gc_block.non_modal_command = NON_MODAL_SET_COORDINATE_OFFSET; break; // G92 + case 100: gc_block.non_modal_command = NON_MODAL_RESET_COORDINATE_OFFSET; break; // G92.1 + default: FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); // [Unsupported G92.x command] + } + mantissa = 0; // Set to zero to indicate valid non-integer G command. + break; } break; - case 38: - int_value = trunc(10*value); // Multiply by 10 to pick up Gxx.1 + case 0: case 1: case 2: case 3: case 38: + // Check for G0/1/2/3/38 being called with G10/28/30/92 on same block. + if (axis_explicit) { FAIL(STATUS_GCODE_AXIS_COMMAND_CONFLICT); } // [Axis word/command conflict] + axis_explicit = AXIS_COMMAND_MOTION_MODE; + // No break. Continues to next line. + case 80: + word_bit = MODAL_GROUP_G1; switch(int_value) { - case 382: gc.motion_mode = MOTION_MODE_PROBE; break; - // case 383: gc.motion_mode = MOTION_MODE_PROBE_NO_ERROR; break; // Not supported. - // case 384: // Not supported. - // case 385: // Not supported. - default: FAIL(STATUS_UNSUPPORTED_STATEMENT); - } - break; - case 53: absolute_override = true; break; - case 54: case 55: case 56: case 57: case 58: case 59: - gc.coord_select = int_value-54; + case 0: gc_block.modal.motion = MOTION_MODE_SEEK; break; // G0 + case 1: gc_block.modal.motion = MOTION_MODE_LINEAR; break; // G1 + case 2: gc_block.modal.motion = MOTION_MODE_CW_ARC; break; // G2 + case 3: gc_block.modal.motion = MOTION_MODE_CCW_ARC; break; // G3 + case 38: + switch(mantissa) { + case 200: gc_block.modal.motion = MOTION_MODE_PROBE; break; // G38.2 + // NOTE: If G38.3+ are enabled, change mantissa variable type to uint16_t. + // case 300: gc_block.modal.motion = MOTION_MODE_PROBE_NO_ERROR; break; // G38.3 Not supported. + // case 400: // Not supported. + // case 500: // Not supported. + default: FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); // [Unsupported G38.x command] + } + mantissa = 0; // Set to zero to indicate valid non-integer G command. + break; + case 80: gc_block.modal.motion = MOTION_MODE_NONE; break; // G80 + } break; - case 80: gc.motion_mode = MOTION_MODE_CANCEL; break; - case 90: gc.absolute_mode = true; break; - case 91: gc.absolute_mode = false; break; - case 92: - int_value = trunc(10*value); // Multiply by 10 to pick up G92.1 + case 17: case 18: case 19: + word_bit = MODAL_GROUP_G2; switch(int_value) { - case 920: non_modal_action = NON_MODAL_SET_COORDINATE_OFFSET; break; - case 921: non_modal_action = NON_MODAL_RESET_COORDINATE_OFFSET; break; - default: FAIL(STATUS_UNSUPPORTED_STATEMENT); + case 17: gc_block.modal.plane_select = PLANE_SELECT_XY; break; + case 18: gc_block.modal.plane_select = PLANE_SELECT_ZX; break; + case 19: gc_block.modal.plane_select = PLANE_SELECT_YZ; break; } break; - case 93: gc.inverse_feed_rate_mode = true; break; - case 94: gc.inverse_feed_rate_mode = false; break; - default: FAIL(STATUS_UNSUPPORTED_STATEMENT); - } - break; + case 90: case 91: + word_bit = MODAL_GROUP_G3; + if (int_value == 90) { gc_block.modal.distance = DISTANCE_MODE_ABSOLUTE; } // G90 + else { gc_block.modal.distance = DISTANCE_MODE_INCREMENTAL; } // G91 + break; + case 93: case 94: + word_bit = MODAL_GROUP_G5; + if (int_value == 93) { gc_block.modal.feed_rate = FEED_RATE_MODE_INVERSE_TIME; } // G93 + else { gc_block.modal.feed_rate = FEED_RATE_MODE_UNITS_PER_MIN; } // G94 + break; + case 20: case 21: + word_bit = MODAL_GROUP_G6; + if (int_value == 20) { gc_block.modal.units = UNITS_MODE_INCHES; } // G20 + else { gc_block.modal.units = UNITS_MODE_MM; } // G21 + break; + case 54: case 55: case 56: case 57: case 58: case 59: + // NOTE: G59.x are not supported. (But their int_values would be 60, 61, and 62.) + word_bit = MODAL_GROUP_G12; + gc_block.modal.coord_select = int_value-54; // Shift to array indexing. + break; + default: FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); // [Unsupported G command] + } + if (mantissa > 0) { FAIL(STATUS_GCODE_COMMAND_VALUE_NOT_INTEGER); } // [Unsupported or invalid Gxx.x command] + // Check for more than one command per modal group violations in the current block + // NOTE: Variable 'word_bit' is always assigned, if the command is valid. + if ( bit_istrue(command_words,bit(word_bit)) ) { FAIL(STATUS_GCODE_MODAL_GROUP_VIOLATION); } + bit_true(command_words,bit(word_bit)); + break; + case 'M': - // Set modal group values + + // Determine 'M' command and its modal group + if (mantissa > 0) { FAIL(STATUS_GCODE_COMMAND_VALUE_NOT_INTEGER); } // [No Mxx.x commands] switch(int_value) { - case 0: case 1: case 2: case 30: group_number = MODAL_GROUP_4; break; - case 3: case 4: case 5: group_number = MODAL_GROUP_7; break; - case 7: case 8: case 9: group_number = MODAL_GROUP_8; break; - } - // Set 'M' commands - switch(int_value) { - case 0: gc.program_flow = PROGRAM_FLOW_PAUSED; break; // Program pause - case 1: break; // Optional stop not supported. Ignore. - case 2: case 30: gc.program_flow = PROGRAM_FLOW_COMPLETED; break; // Program end and reset - case 3: gc.spindle_direction = SPINDLE_ENABLE_CW; break; - case 4: gc.spindle_direction = SPINDLE_ENABLE_CCW; break; - case 5: gc.spindle_direction = SPINDLE_DISABLE; break; - #ifdef ENABLE_M7 - case 7: gc.coolant_mode = COOLANT_MIST_ENABLE; break; - #endif - case 8: gc.coolant_mode = COOLANT_FLOOD_ENABLE; break; - case 9: gc.coolant_mode = COOLANT_DISABLE; break; - default: FAIL(STATUS_UNSUPPORTED_STATEMENT); + case 0: case 1: case 2: case 30: + word_bit = MODAL_GROUP_M4; + switch(int_value) { + case 0: gc_block.modal.program_flow = PROGRAM_FLOW_PAUSED; break; // Program pause + case 1: break; // Optional stop not supported. Ignore. + case 2: case 30: gc_block.modal.program_flow = PROGRAM_FLOW_COMPLETED; break; // Program end and reset + } + break; + case 3: case 4: case 5: + word_bit = MODAL_GROUP_M7; + switch(int_value) { + case 3: gc_block.modal.spindle = SPINDLE_ENABLE_CW; break; + case 4: gc_block.modal.spindle = SPINDLE_ENABLE_CCW; break; + case 5: gc_block.modal.spindle = SPINDLE_DISABLE; break; + } + break; + #ifdef ENABLE_M7 + case 7: + #endif + case 8: case 9: + word_bit = MODAL_GROUP_M8; + switch(int_value) { + #ifdef ENABLE_M7 + case 7: gc_block.modal.coolant = COOLANT_MIST_ENABLE; break; + #endif + case 8: gc_block.modal.coolant = COOLANT_FLOOD_ENABLE; break; + case 9: gc_block.modal.coolant = COOLANT_DISABLE; break; + } + break; + default: FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); // [Unsupported M command] } + + // Check for more than one command per modal group violations in the current block + // NOTE: Variable 'word_bit' is always assigned, if the command is valid. + if ( bit_istrue(command_words,bit(word_bit)) ) { FAIL(STATUS_GCODE_MODAL_GROUP_VIOLATION); } + bit_true(command_words,bit(word_bit)); break; - } - // Check for modal group multiple command violations in the current block - if (group_number) { - if ( bit_istrue(modal_group_words,bit(group_number)) ) { - FAIL(STATUS_MODAL_GROUP_VIOLATION); - } else { - bit_true(modal_group_words,bit(group_number)); - } - group_number = MODAL_GROUP_NONE; // Reset for next command. - } - } - - // If there were any errors parsing this line, we will return right away with the bad news - if (gc.status_code) { return(gc.status_code); } + + // NOTE: All remaining letters assign values. + default: - /* Pass 2: Parameters. All units converted according to current block commands. Position - parameters are converted and flagged to indicate a change. These can have multiple connotations - for different commands. Each will be converted to their proper value upon execution. - NOTE: Grbl unconventionally pre-converts these parameter values based on the block G and M - commands. This is set out of the order of execution defined by NIST only for code efficiency/size - purposes, but should not affect proper g-code execution. */ - float p = 0; - uint8_t l = 0; - char_counter = 0; - while(next_statement(&letter, &value, line, &char_counter)) { - switch(letter) { - case 'G': case 'M': break; // Ignore command statements and line numbers - case 'F': - if (value <= 0) { FAIL(STATUS_INVALID_STATEMENT); } // Must be greater than zero - if (gc.inverse_feed_rate_mode) { - inverse_feed_rate = to_millimeters(value); // seconds per motion for this motion only - } else { - gc.feed_rate = to_millimeters(value); // millimeters per minute + /* Non-Command Words: This initial parsing phase only checks for repeats of the remaining + legal g-code words and stores their value. Error-checking is performed later since some + words (I,J,K,L,P,R) have multiple connotations and/or depend on the issued commands. */ + switch(letter){ + // case 'A': // Not supported + // case 'B': // Not supported + // case 'C': // Not supported + // case 'D': // Not supported + case 'F': word_bit = WORD_F; gc_block.values.f = value; break; + // case 'H': // Not supported + case 'I': word_bit = WORD_I; gc_block.values.ijk[X_AXIS] = value; ijk_words |= (1< MAX_LINE_NUMBER) { FAIL(STATUS_GCODE_INVALID_LINE_NUMBER); } // [Exceeds max line number] } + // bit_false(value_words,bit(WORD_N)); // NOTE: Single-meaning value word. Set at end of error-checking. - // [G4,G10,G28,G30,G92,G92.1]: Perform dwell, set coordinate system data, homing, or set axis offsets. - // NOTE: These commands are in the same modal group, hence are mutually exclusive. G53 is in this - // modal group and do not effect these actions. - switch (non_modal_action) { - case NON_MODAL_DWELL: - if (p < 0) { // Time cannot be negative. - FAIL(STATUS_INVALID_STATEMENT); + // Track for unused words at the end of error-checking. + // NOTE: Single-meaning value words are removed all at once at the end of error-checking, because + // they are always used when present. This was done to save a few bytes of flash. For clarity, the + // single-meaning value words may be removed as they are used. Also, axis words are treated in the + // same way. If there is an axis explicit/implicit command, XYZ words are always used and are + // are removed at the end of error-checking. + + // [1. Comments ]: MSG's NOT SUPPORTED. Comment handling performed by protocol. + + // [2. Set feed rate mode ]: G93 F word missing with G1,G2/3 active, implicitly or explicitly. Feed rate + // is not defined after switching to G94 from G93. + if (gc_block.modal.feed_rate == FEED_RATE_MODE_INVERSE_TIME) { // = G93 + // NOTE: G38 can also operate in inverse time, but is undefined as an error. Missing F word check added here. + if (axis_explicit == AXIS_COMMAND_MOTION_MODE) { + if ((gc_block.modal.motion != MOTION_MODE_NONE) || (gc_block.modal.motion != MOTION_MODE_SEEK)) { + if (bit_isfalse(value_words,bit(WORD_F))) { FAIL(STATUS_GCODE_UNDEFINED_FEED_RATE); } // [F word missing] + } + } + // NOTE: It seems redundant to check for an F word to be passed after switching from G94 to G93. We would + // accomplish the exact same thing if the feed rate value is always reset to zero and undefined after each + // inverse time block, since the commands that use this value already perform undefined checks. This would + // also allow other commands, following this switch, to execute and not error out needlessly. This code is + // combined with the above feed rate mode and the below set feed rate error-checking. + + // [3. Set feed rate ]: F is negative (done.) + // - In inverse time mode: Always implicitly zero the feed rate value before and after block completion. + // NOTE: If in G93 mode or switched into it from G94, just keep F value as initialized zero or passed F word + // value in the block. If no F word is passed with a motion command that requires a feed rate, this will error + // out in the motion modes error-checking. However, if no F word is passed with NO motion command that requires + // a feed rate, we simply move on and the state feed rate value gets updated to zero and remains undefined. + } else { // = G94 + // - In units per mm mode: If F word passed, ensure value is in mm/min, otherwise push last state value. + if (gc_state.modal.feed_rate == FEED_RATE_MODE_UNITS_PER_MIN) { // Last state is also G94 + if (bit_istrue(value_words,bit(WORD_F))) { + if (gc_block.modal.units == UNITS_MODE_INCHES) { gc_block.values.f *= MM_PER_INCH; } } else { - // Ignore dwell in check gcode modes - if (sys.state != STATE_CHECK_MODE) { mc_dwell(p); } + gc_block.values.f = gc_state.feed_rate; // Push last state feed rate + } + } // Else, switching to G94 from G93, so don't push last state feed rate. Its undefined or the passed F word value. + } + // bit_false(value_words,bit(WORD_F)); // NOTE: Single-meaning value word. Set at end of error-checking. + + // [4. Set spindle speed ]: S is negative (done.) + if (bit_isfalse(value_words,bit(WORD_S))) { gc_block.values.s = gc_state.spindle_speed; } + // bit_false(value_words,bit(WORD_S)); // NOTE: Single-meaning value word. Set at end of error-checking. + + // [5. Select tool ]: NOT SUPPORTED. T is negative (done.) Not an integer. Greater than max tool value. + // bit_false(value_words,bit(WORD_T)); // NOTE: Single-meaning value word. Set at end of error-checking. + + // [6. Change tool ]: N/A + // [7. Spindle control ]: N/A + // [8. Coolant control ]: N/A + // [9. Enable/disable feed rate or spindle overrides ]: NOT SUPPORTED. + + // [10. Dwell ]: P value missing. P is negative (done.) NOTE: See below. + if (gc_block.non_modal_command == NON_MODAL_DWELL) { + if (bit_isfalse(value_words,bit(WORD_P))) { FAIL(STATUS_GCODE_VALUE_WORD_MISSING); } // [P word missing] + bit_false(value_words,bit(WORD_P)); + } + + // [11. Set active plane ]: N/A + switch (gc_block.modal.plane_select) { + case PLANE_SELECT_XY: + axis_0 = X_AXIS; + axis_1 = Y_AXIS; + axis_linear = Z_AXIS; + break; + case PLANE_SELECT_ZX: + axis_0 = Z_AXIS; + axis_1 = X_AXIS; + axis_linear = Y_AXIS; + break; + default: // case PLANE_SELECT_YZ: + axis_0 = Y_AXIS; + axis_1 = Z_AXIS; + axis_linear = X_AXIS; + } + + // [12. Set length units ]: N/A + // Pre-convert XYZ coordinate values to millimeters, if applicable. + uint8_t idx; + if (gc_block.modal.units == UNITS_MODE_INCHES) { + for (idx=0; idx N_COORDINATE_SYSTEM) { FAIL(STATUS_GCODE_UNSUPPORTED_COORD_SYS); } // [Greater than N sys] + if (gc_state.modal.coord_select != gc_block.modal.coord_select) { + if (!(settings_read_coord_data(gc_block.modal.coord_select,coordinate_data))) { FAIL(STATUS_SETTING_READ_FAIL); } + } + } + + // [16. Set path control mode ]: NOT SUPPORTED. + // [17. Set distance mode ]: N/A. G90.1 and G91.1 NOT SUPPORTED. + // [18. Set retract mode ]: NOT SUPPORTED. + + // [19. Remaining non-modal actions ]: Check go to predefined position, set G10, or set axis offsets. + // NOTE: We need to separate the non-modal commands that are axis-explicit (G10/G28/G30/G92), as these + // commands all treat axis words differently. G10 as absolute offsets or computes current position as + // the axis value, G92 similarly to G10 L20, and G28/30 as an intermediate target position that observes + // all the current coordinate system and G92 offsets. + switch (gc_block.non_modal_command) { + case NON_MODAL_SET_COORDINATE_DATA: + // [G10 Errors]: L missing and is not 2 or 20. P word missing. (Negative P value done.) + // [G10 L2 Errors]: R word NOT SUPPORTED. P value not 0 to nCoordSys(max 9). Axis words missing (done.) + // [G10 L20 Errors]: P must be 0 to nCoordSys(max 9). Axis words missing (done.) + if (bit_isfalse(value_words,((1< N_COORDINATE_SYSTEM) { FAIL(STATUS_GCODE_UNSUPPORTED_COORD_SYS); } // [Greater than N sys] + if (gc_block.values.l != 20) { + if (gc_block.values.l == 2) { + if (bit_istrue(value_words,bit(WORD_R))) { FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); } // [G10 L2 R not supported] + } else { FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); } // [Unsupported L] + } + bit_false(value_words,(bit(WORD_L)|bit(WORD_P))); + + // Load EEPROM coordinate data and pre-calculate the new coordinate data. + if (int_value > 0) { int_value--; } // Adjust P1-P6 index to EEPROM coordinate data indexing. + else { int_value = gc_state.modal.coord_select; } // Index P0 as the active coordinate system + if (!settings_read_coord_data(int_value,parameter_data)) { FAIL(STATUS_SETTING_READ_FAIL); } // [EEPROM read fail] + for (idx=0; idx N_COORDINATE_SYSTEM)) { // L2 and L20. P1=G54, P2=G55, ... - FAIL(STATUS_UNSUPPORTED_STATEMENT); - } else if (!axis_words && l==2) { // No axis words. - FAIL(STATUS_INVALID_STATEMENT); - } else { - if (int_value > 0) { int_value--; } // Adjust P1-P6 index to EEPROM coordinate data indexing. - else { int_value = gc.coord_select; } // Index P0 as the active coordinate system - float coord_data[N_AXIS]; - if (!settings_read_coord_data(int_value,coord_data)) { return(STATUS_SETTING_READ_FAIL); } - // Update axes defined only in block. Always in machine coordinates. Can change non-active system. - for (idx=0; idx C -----------------+--------------- T <- [x,y] + | <------ d/2 ---->| + + C - Current position + T - Target position + O - center of circle that pass through both C and T + d - distance from C to T + r - designated radius + h - distance from center of CT to O + + Expanding the equations: + + d -> sqrt(x^2 + y^2) + h -> sqrt(4 * r^2 - x^2 - y^2)/2 + i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 + j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 + + Which can be written: + + i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2 + j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2 + + Which we for size and speed reasons optimize to: + + h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2) + i = (x - (y * h_x2_div_d))/2 + j = (y + (x * h_x2_div_d))/2 + */ + + // First, use h_x2_div_d to compute 4*h^2 to check if it is negative or r is smaller + // than d. If so, the sqrt of a negative number is complex and error out. + float h_x2_div_d = 4.0 * gc_block.values.r*gc_block.values.r - x*x - y*y; + + if (h_x2_div_d < 0) { FAIL(STATUS_GCODE_ARC_RADIUS_ERROR); } // [Arc radius error] + + // Finish computing h_x2_div_d. + h_x2_div_d = -sqrt(h_x2_div_d)/hypot_f(x,y); // == -(h * 2 / d) + // Invert the sign of h_x2_div_d if the circle is counter clockwise (see sketch below) + if (gc_block.modal.motion == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d; } + + /* The counter clockwise circle lies to the left of the target direction. When offset is positive, + the left hand circle will be generated - when it is negative the right hand circle is generated. + + T <-- Target position + + ^ + Clockwise circles with this center | Clockwise circles with this center will have + will have > 180 deg of angular travel | < 180 deg of angular travel, which is a good thing! + \ | / + center of arc when h_x2_div_d is positive -> x <----- | -----> x <- center of arc when h_x2_div_d is negative + | + | + + C <-- Current position + */ + // Negative R is g-code-alese for "I want a circle with more than 180 degrees of travel" (go figure!), + // 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 (gc_block.values.r < 0) { + h_x2_div_d = -h_x2_div_d; + gc_block.values.r = -gc_block.values.r; // Finished with r. Set to positive for mc_arc + } + // Complete the operation by calculating the actual center of the arc + gc_block.values.ijk[axis_0] = 0.5*(x-(y*h_x2_div_d)); + gc_block.values.ijk[axis_1] = 0.5*(y+(x*h_x2_div_d)); + + } else { // Arc Center Format Offset Mode + if (!(ijk_words & (bit(axis_0)|bit(axis_1)))) { FAIL(STATUS_GCODE_NO_OFFSETS_IN_PLANE); } // [No offsets in plane] + bit_false(value_words,(bit(WORD_I)|bit(WORD_J)|bit(WORD_K))); + + // Convert IJK values to proper units. + if (gc_block.modal.units == UNITS_MODE_INCHES) { + for (idx=0; idx 0.002) { FAIL(STATUS_GCODE_INVALID_TARGET); } // [Arc definition error] + } + break; + case MOTION_MODE_PROBE: + // [G38 Errors]: Target is same current. No axis words. Cutter compensation is enabled. Feed rate + // is undefined. Probe is triggered. + if (gc_check_same_position(gc_state.position, gc_block.values.xyz)) { FAIL(STATUS_GCODE_INVALID_TARGET); } // [Invalid target] + if (probe_get_state()) { FAIL(STATUS_GCODE_PROBE_TRIGGERED); } // [Probe triggered] + break; + } + } + } + + // [21. Program flow ]: No error check required. + + // [0. Non-specific error-checks]: Complete unused value words check, i.e. IJK used when in arc + // radius mode, or axis words that aren't used in the block. + bit_false(value_words,(bit(WORD_N)|bit(WORD_F)|bit(WORD_S)|bit(WORD_T))); // Remove single-meaning value words. + if (axis_explicit) { bit_false(value_words,(bit(WORD_X)|bit(WORD_Y)|bit(WORD_Z))); } // Remove axis words. + if (value_words) { FAIL(STATUS_GCODE_UNUSED_WORDS); } // [Unused words] + + + /* ------------------------------------------------------------------------------------- + STEP 4: EXECUTE!! + Assumes that all error-checking has been completed and no failure modes exist. We just + need to update the state and execute the block according to the order-of-execution. + */ + + // [1. Comments feedback ]: NOT SUPPORTED + + // [2. Set feed rate mode ]: + gc_state.modal.feed_rate = gc_block.modal.feed_rate; + + // [3. Set feed rate ]: + gc_state.feed_rate = gc_block.values.f; // Always copy this value. See feed rate error-checking. + + // [4. Set spindle speed ]: + if (gc_state.spindle_speed != gc_block.values.s) { + gc_state.spindle_speed = gc_block.values.s; + + // Update running spindle only if not in check mode and not already enabled. + if (gc_state.modal.spindle != SPINDLE_DISABLE) { spindle_run(gc_state.modal.spindle, gc_state.spindle_speed); } + } + + // [5. Select tool ]: NOT SUPPORTED + + // [6. Change tool ]: NOT SUPPORTED + + // [7. Spindle control ]: + if (gc_state.modal.spindle != gc_block.modal.spindle) { + gc_state.modal.spindle = gc_block.modal.spindle; + // Update spindle control and apply spindle speed when enabling it in this block. + spindle_run(gc_state.modal.spindle, gc_state.spindle_speed); + } + + // [8. Coolant control ]: + if (gc_state.modal.coolant != gc_block.modal.coolant) { + gc_state.modal.coolant = gc_block.modal.coolant; + coolant_run(gc_state.modal.coolant); + } + + // [9. Enable/disable feed rate or spindle overrides ]: NOT SUPPORTED + + // [10. Dwell ]: + if (gc_block.non_modal_command == NON_MODAL_DWELL) { mc_dwell(gc_block.values.p); } + + // [11. Set active plane ]: + gc_state.modal.plane_select = gc_block.modal.plane_select; + + // [12. Set length units ]: + gc_state.modal.units = gc_block.modal.units; + + // [13. Cutter radius compensation ]: NOT SUPPORTED + + // [14. Cutter length compensation ]: NOT SUPPORTED + + // [15. Coordinate system selection ]: + if (gc_state.modal.coord_select != gc_block.modal.coord_select) { + gc_state.modal.coord_select = gc_block.modal.coord_select; + memcpy(gc_state.coord_system,coordinate_data,sizeof(coordinate_data)); + } + + // [16. Set path control mode ]: NOT SUPPORTED + + // [17. Set distance mode ]: + gc_state.modal.distance = gc_block.modal.distance; + + // [18. Set retract mode ]: NOT SUPPORTED + + // [19. Go to predefined position, Set G10, or Set axis offsets ]: + switch(gc_block.non_modal_command) { + case NON_MODAL_SET_COORDINATE_DATA: + +// TODO: See if I can clean up this int_value. + int_value = trunc(gc_block.values.p); // Convert p value to int. + if (int_value > 0) { int_value--; } // Adjust P1-P6 index to EEPROM coordinate data indexing. + else { int_value = gc_state.modal.coord_select; } // Index P0 as the active coordinate system + + settings_write_coord_data(int_value,parameter_data); + // Update system coordinate system if currently active. + if (gc_state.modal.coord_select == int_value) { memcpy(gc_state.coord_system,parameter_data,sizeof(parameter_data)); } break; case NON_MODAL_GO_HOME_0: case NON_MODAL_GO_HOME_1: // Move to intermediate position before going home. Obeys current coordinate system and offsets // and absolute and incremental modes. - if (axis_words) { - // Apply absolute mode coordinate offsets or incremental mode offsets. - for (idx=0; idx 'Z')) { - FAIL(STATUS_EXPECTED_COMMAND_LETTER); - return(0); - } - (*char_counter)++; - if (!read_float(line, char_counter, float_ptr)) { - FAIL(STATUS_BAD_NUMBER_FORMAT); - return(0); - }; - return(1); -} - - -static void gc_convert_arc_radius_mode(float *target) -{ -/* We need to calculate the center of the circle that has the designated radius and passes - through both the current position and the target position. This method calculates the following - set of equations where [x,y] is the vector from current to target position, d == magnitude of - that vector, h == hypotenuse of the triangle formed by the radius of the circle, the distance to - the center of the travel vector. A vector perpendicular to the travel vector [-y,x] is scaled to the - length of h [-y/d*h, x/d*h] and added to the center of the travel vector [x/2,y/2] to form the new point - [i,j] at [x/2-y/d*h, y/2+x/d*h] which will be the center of our arc. - - d^2 == x^2 + y^2 - h^2 == r^2 - (d/2)^2 - i == x/2 - y/d*h - j == y/2 + x/d*h - - O <- [i,j] - - | - r - | - - | - - | h - - | - [0,0] -> C -----------------+--------------- T <- [x,y] - | <------ d/2 ---->| - - C - Current position - T - Target position - O - center of circle that pass through both C and T - d - distance from C to T - r - designated radius - h - distance from center of CT to O - - Expanding the equations: - - d -> sqrt(x^2 + y^2) - h -> sqrt(4 * r^2 - x^2 - y^2)/2 - i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 - j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 - - Which can be written: - - i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2 - j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2 - - Which we for size and speed reasons optimize to: - - h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2) - i = (x - (y * h_x2_div_d))/2 - j = (y + (x * h_x2_div_d))/2 */ - - // Calculate the change in position along each selected axis - float x = target[gc.plane_axis_0]-gc.position[gc.plane_axis_0]; - float y = target[gc.plane_axis_1]-gc.position[gc.plane_axis_1]; - - clear_vector(gc.arc_offset); - // First, use h_x2_div_d to compute 4*h^2 to check if it is negative or r is smaller - // than d. If so, the sqrt of a negative number is complex and error out. - float h_x2_div_d = 4 * gc.arc_radius*gc.arc_radius - x*x - y*y; - if (h_x2_div_d < 0) { FAIL(STATUS_ARC_RADIUS_ERROR); return; } - // Finish computing h_x2_div_d. - h_x2_div_d = -sqrt(h_x2_div_d)/hypot(x,y); // == -(h * 2 / d) - // Invert the sign of h_x2_div_d if the circle is counter clockwise (see sketch below) - if (gc.motion_mode == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d; } - - /* The counter clockwise circle lies to the left of the target direction. When offset is positive, - the left hand circle will be generated - when it is negative the right hand circle is generated. - - - T <-- Target position - - ^ - Clockwise circles with this center | Clockwise circles with this center will have - will have > 180 deg of angular travel | < 180 deg of angular travel, which is a good thing! - \ | / - center of arc when h_x2_div_d is positive -> x <----- | -----> x <- center of arc when h_x2_div_d is negative - | - | - - C <-- Current position */ - - - // Negative R is g-code-alese for "I want a circle with more than 180 degrees of travel" (go figure!), - // 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 (gc.arc_radius < 0) { - h_x2_div_d = -h_x2_div_d; - gc.arc_radius = -gc.arc_radius; // Finished with r. Set to positive for mc_arc - } - // Complete the operation by calculating the actual center of the arc - gc.arc_offset[gc.plane_axis_0] = 0.5*(x-(y*h_x2_div_d)); - gc.arc_offset[gc.plane_axis_1] = 0.5*(y+(x*h_x2_div_d)); -} + /* Not supported: @@ -648,14 +944,13 @@ static void gc_convert_arc_radius_mode(float *target) - A,B,C-axes - Evaluation of expressions - Variables - - Probing - Override control (TBD) - Tool changes - Switches (*) Indicates optional parameter, enabled through config.h and re-compile group 0 = {G92.2, G92.3} (Non modal: Cancel and re-enable G92 offsets) - group 1 = {G38.2, G81 - G89} (Motion modes: straight probe, canned cycles) + group 1 = {G81 - G89} (Motion modes: Canned cycles) group 4 = {M1} (Optional stop, ignored) group 6 = {M6} (Tool change) group 8 = {*M7} enable mist coolant diff --git a/gcode.h b/gcode.h index acd7046..3acba8f 100644 --- a/gcode.h +++ b/gcode.h @@ -29,68 +29,151 @@ // mutually exclusive, or cannot exist on the same line, because they each toggle a state or execute // a unique motion. These are defined in the NIST RS274-NGC v3 g-code standard, available online, // and are similar/identical to other g-code interpreters by manufacturers (Haas,Fanuc,Mazak,etc). -#define MODAL_GROUP_NONE 0 -#define MODAL_GROUP_0 1 // [G4,G10,G28,G30,G53,G92,G92.1] Non-modal -#define MODAL_GROUP_1 2 // [G0,G1,G2,G3,G38.2,G80] Motion -#define MODAL_GROUP_2 3 // [G17,G18,G19] Plane selection -#define MODAL_GROUP_3 4 // [G90,G91] Distance mode -#define MODAL_GROUP_4 5 // [M0,M1,M2,M30] Stopping -#define MODAL_GROUP_5 6 // [G93,G94] Feed rate mode -#define MODAL_GROUP_6 7 // [G20,G21] Units -#define MODAL_GROUP_7 8 // [M3,M4,M5] Spindle turning -#define MODAL_GROUP_8 9 // [M7,M8,M9] Coolant control -#define MODAL_GROUP_12 10 // [G54,G55,G56,G57,G58,G59] Coordinate system selection +// NOTE: Modal group define values must be sequential and starting from zero. +#define MODAL_GROUP_G0 0 // [G4,G10,G28,G28.1,G30,G30.1,G53,G92,G92.1] Non-modal +#define MODAL_GROUP_G1 1 // [G0,G1,G2,G3,G38.2,G80] Motion +#define MODAL_GROUP_G2 2 // [G17,G18,G19] Plane selection +#define MODAL_GROUP_G3 3 // [G90,G91] Distance mode +#define MODAL_GROUP_G5 4 // [G93,G94] Feed rate mode +#define MODAL_GROUP_G6 5 // [G20,G21] Units +#define MODAL_GROUP_G12 6 // [G54,G55,G56,G57,G58,G59] Coordinate system selection + +#define MODAL_GROUP_M4 7 // [M0,M1,M2,M30] Stopping +#define MODAL_GROUP_M7 8 // [M3,M4,M5] Spindle turning +#define MODAL_GROUP_M8 9 // [M7,M8,M9] Coolant control + +#define OTHER_INPUT_F 10 +#define OTHER_INPUT_S 11 +#define OTHER_INPUT_T 12 // Define command actions for within execution-type modal groups (motion, stopping, non-modal). Used // internally by the parser to know which command to execute. -#define MOTION_MODE_SEEK 0 // G0 -#define MOTION_MODE_LINEAR 1 // G1 -#define MOTION_MODE_CW_ARC 2 // G2 -#define MOTION_MODE_CCW_ARC 3 // G3 -#define MOTION_MODE_PROBE 4 // G38.x -#define MOTION_MODE_CANCEL 5 // G80 -#define PROGRAM_FLOW_RUNNING 0 -#define PROGRAM_FLOW_PAUSED 1 // M0, M1 -#define PROGRAM_FLOW_COMPLETED 2 // M2, M30 - -#define NON_MODAL_NONE 0 +// Modal Group 0: Non-modal actions +#define NON_MODAL_NO_ACTION 0 // (Default: Must be zero) #define NON_MODAL_DWELL 1 // G4 #define NON_MODAL_SET_COORDINATE_DATA 2 // G10 #define NON_MODAL_GO_HOME_0 3 // G28 #define NON_MODAL_SET_HOME_0 4 // G28.1 #define NON_MODAL_GO_HOME_1 5 // G30 #define NON_MODAL_SET_HOME_1 6 // G30.1 -#define NON_MODAL_SET_COORDINATE_OFFSET 7 // G92 -#define NON_MODAL_RESET_COORDINATE_OFFSET 8 //G92.1 +#define NON_MODAL_ABSOLUTE_OVERRIDE 7 // G53 +#define NON_MODAL_SET_COORDINATE_OFFSET 8 // G92 +#define NON_MODAL_RESET_COORDINATE_OFFSET 9 //G92.1 + +// Modal Group 1: Motion modes +#define MOTION_MODE_SEEK 0 // G0 (Default: Must be zero) +#define MOTION_MODE_LINEAR 1 // G1 +#define MOTION_MODE_CW_ARC 2 // G2 +#define MOTION_MODE_CCW_ARC 3 // G3 +#define MOTION_MODE_PROBE 4 // G38.2 +#define MOTION_MODE_NONE 5 // G80 + +// Modal Group 2: Plane select +#define PLANE_SELECT_XY 0 // G17 (Default: Must be zero) +#define PLANE_SELECT_ZX 1 // G18 +#define PLANE_SELECT_YZ 2 // G19 + +// Modal Group 3: Distance mode +#define DISTANCE_MODE_ABSOLUTE 0 // G90 (Default: Must be zero) +#define DISTANCE_MODE_INCREMENTAL 1 // G91 + +// Modal Group 4: Program flow +#define PROGRAM_FLOW_RUNNING 0 // (Default: Must be zero) +#define PROGRAM_FLOW_PAUSED 1 // M0, M1 +#define PROGRAM_FLOW_COMPLETED 2 // M2, M30 + +// Modal Group 5: Feed rate mode +#define FEED_RATE_MODE_UNITS_PER_MIN 0 // G94 (Default: Must be zero) +#define FEED_RATE_MODE_INVERSE_TIME 1 // G93 + +// Modal Group 6: Units mode +#define UNITS_MODE_MM 0 // G21 (Default: Must be zero) +#define UNITS_MODE_INCHES 1 // G20 + +// Modal Group 7: Spindle control +#define SPINDLE_DISABLE 0 // M5 (Default: Must be zero) +#define SPINDLE_ENABLE_CW 1 // M3 +#define SPINDLE_ENABLE_CCW 2 // M4 + +// Modal Group 8: Coolant control +#define COOLANT_DISABLE 0 // M9 (Default: Must be zero) +#define COOLANT_MIST_ENABLE 1 // M7 +#define COOLANT_FLOOD_ENABLE 2 // M8 + +// Modal Group 12: Active work coordinate system +// N/A: Stores coordinate system value (54-59) to change to. + +#define WORD_F 0 +#define WORD_I 1 +#define WORD_J 2 +#define WORD_K 3 +#define WORD_L 4 +#define WORD_N 5 +#define WORD_P 6 +#define WORD_R 7 +#define WORD_S 8 +#define WORD_T 9 +#define WORD_X 10 +#define WORD_Y 11 +#define WORD_Z 12 + + + + +// NOTE: When this struct is zeroed, the above defines set the defaults for the system. +typedef struct { + uint8_t motion; // {G0,G1,G2,G3,G80} + uint8_t feed_rate; // {G93,G94} + uint8_t units; // {G20,G21} + uint8_t distance; // {G90,G91} + uint8_t plane_select; // {G17,G18,G19} + uint8_t coord_select; // {G54,G55,G56,G57,G58,G59} + uint8_t program_flow; // {M0,M1,M2,M30} + uint8_t coolant; // {M7,M8,M9} + uint8_t spindle; // {M3,M4,M5} +} gc_modal_t; typedef struct { - uint8_t status_code; // Parser status for current block - uint8_t motion_mode; // {G0, G1, G2, G3, G80} - uint8_t inverse_feed_rate_mode; // {G93, G94} - uint8_t inches_mode; // 0 = millimeter mode, 1 = inches mode {G20, G21} - uint8_t absolute_mode; // 0 = relative motion, 1 = absolute motion {G90, G91} - uint8_t program_flow; // {M0, M1, M2, M30} - uint8_t coolant_mode; // 0 = Disable, 1 = Flood Enable, 2 = Mist Enable {M8, M9} - uint8_t spindle_direction; // 1 = CW, 2 = CCW, 0 = Stop {M3, M4, M5} + float f; // Feed + float ijk[3]; // I,J,K Axis arc offsets + uint8_t l; // G10 or canned cycles parameters + int32_t n; // Line number + float p; // G10 or dwell parameters + // float q; // G82 peck drilling + float r; // Arc radius + float s; // Spindle speed + // uint8_t t; // Tool selection + float xyz[3]; // X,Y,Z Translational axes +} gc_values_t; + + +typedef struct { + gc_modal_t modal; + float spindle_speed; // RPM float feed_rate; // Millimeters/min - float position[N_AXIS]; // Where the interpreter considers the tool to be at this point in the code uint8_t tool; - uint8_t plane_axis_0, - plane_axis_1, - plane_axis_2; // The axes of the selected plane - uint8_t coord_select; // Active work coordinate system number. Default: 0=G54. + + float position[N_AXIS]; // Where the interpreter considers the tool to be at this point in the code + float coord_system[N_AXIS]; // Current work coordinate system (G54+). Stores offset from absolute machine - // position in mm. Loaded from EEPROM when called. + // position in mm. Loaded from EEPROM when called. float coord_offset[N_AXIS]; // Retains the G92 coordinate offset (work coordinates) relative to // machine zero in mm. Non-persistent. Cleared upon reset and boot. - - float arc_radius; - float arc_offset[N_AXIS]; - } parser_state_t; -extern parser_state_t gc; +extern parser_state_t gc_state; + +typedef struct { +// uint16_t command_words; // NOTE: If this bitflag variable fills, G and M words can be separated. +// uint16_t value_words; + + uint8_t non_modal_command; + gc_modal_t modal; + gc_values_t values; + +} parser_block_t; +extern parser_block_t gc_block; // Initialize the parser void gc_init(); diff --git a/motion_control.c b/motion_control.c index 6fda0c2..321dfe9 100644 --- a/motion_control.c +++ b/motion_control.c @@ -97,16 +97,15 @@ void mc_line(float *target, float feed_rate, uint8_t invert_feed_rate) // 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. #ifdef USE_LINE_NUMBERS -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, int32_t line_number) +void mc_arc(float *position, float *target, float *offset, float radius, float feed_rate, + uint8_t invert_feed_rate, uint8_t axis_0, uint8_t axis_1, uint8_t axis_linear, int32_t line_number) #else -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) +void mc_arc(float *position, float *target, float *offset, float radius, float feed_rate, + uint8_t invert_feed_rate, uint8_t axis_0, uint8_t axis_1, uint8_t axis_linear) #endif -{ +{ float center_axis0 = position[axis_0] + offset[axis_0]; float center_axis1 = position[axis_1] + offset[axis_1]; - float linear_travel = target[axis_linear] - position[axis_linear]; float r_axis0 = -offset[axis_0]; // Radius vector from center to current location float r_axis1 = -offset[axis_1]; float rt_axis0 = target[axis_0] - center_axis0; @@ -114,7 +113,7 @@ void mc_arc(float *position, float *target, float *offset, uint8_t axis_0, uint8 // CCW angle between position and target from circle center. Only one atan2() trig computation required. float angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1); - if (isclockwise) { // Correct atan2 output per direction + if (gc_state.modal.motion == MOTION_MODE_CW_ARC) { // Correct atan2 output per direction if (angular_travel >= 0) { angular_travel -= 2*M_PI; } } else { if (angular_travel <= 0) { angular_travel += 2*M_PI; } @@ -123,10 +122,8 @@ void mc_arc(float *position, float *target, float *offset, uint8_t axis_0, uint8 // 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)); - uint16_t segments = floor(0.5*millimeters_of_travel/ + // For the intended uses of Grbl, this value shouldn't exceed 2000 for the strictest of cases. + uint16_t segments = floor(fabs(0.5*angular_travel*radius)/ sqrt(settings.arc_tolerance*(2*radius - settings.arc_tolerance)) ); if (segments) { @@ -136,7 +133,7 @@ void mc_arc(float *position, float *target, float *offset, uint8_t axis_0, uint8 if (invert_feed_rate) { feed_rate *= segments; } float theta_per_segment = angular_travel/segments; - float linear_per_segment = linear_travel/segments; + float linear_per_segment = (target[axis_linear] - position[axis_linear])/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. @@ -166,17 +163,13 @@ void mc_arc(float *position, float *target, float *offset, uint8_t axis_0, uint8 float sin_T = theta_per_segment*0.16666667*(cos_T + 4.0); cos_T *= 0.5; - float arc_target[N_AXIS]; float sin_Ti; float cos_Ti; float r_axisi; uint16_t i; uint8_t count = 0; - // Initialize the linear axis - arc_target[axis_linear] = position[axis_linear]; - - for (i = 1; i (b)) ? (a) : (b)) #define min(a,b) (((a) < (b)) ? (a) : (b)) @@ -53,7 +53,7 @@ // 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 float_ptr is // a pointer to the result variable. Returns true when it succeeds -int read_float(char *line, uint8_t *char_counter, float *float_ptr); +uint8_t read_float(char *line, uint8_t *char_counter, float *float_ptr); // Delays variable-defined milliseconds. Compiler compatibility fix for _delay_ms(). void delay_ms(uint16_t ms); @@ -63,4 +63,6 @@ void delay_us(uint32_t us); uint8_t get_direction_mask(uint8_t i); +float hypot_f(float x, float y); + #endif diff --git a/print.c b/print.c index 03e261d..6fe8c2d 100644 --- a/print.c +++ b/print.c @@ -76,32 +76,52 @@ void print_uint8_base2(uint8_t n) serial_write('0' + buf[i - 1]); } -void print_uint32_base10(unsigned long n) +void print_uint8_base10(uint8_t n) { - unsigned char buf[10]; - uint8_t i = 0; - if (n == 0) { serial_write('0'); return; } + + unsigned char buf[3]; + uint8_t i = 0; + + while (n > 0) { + buf[i++] = n % 10 + '0'; + n /= 10; + } + + for (; i > 0; i--) + serial_write(buf[i - 1]); +} + +void print_uint32_base10(unsigned long n) +{ + if (n == 0) { + serial_write('0'); + return; + } + + unsigned char buf[10]; + uint8_t i = 0; while (n > 0) { - buf[i++] = n % 10 + '0'; + buf[i++] = n % 10; n /= 10; } for (; i > 0; i--) - serial_write(buf[i-1]); + serial_write('0' + buf[i-1]); } void printInteger(long n) { if (n < 0) { serial_write('-'); - n = -n; + print_uint32_base10((-n)); + } else { + print_uint32_base10(n); } - print_uint32_base10(n); } // Convert float to string by immediately converting to a long integer, which contains diff --git a/print.h b/print.h index ecd4238..273a554 100644 --- a/print.h +++ b/print.h @@ -35,6 +35,8 @@ void print_uint32_base10(uint32_t n); void print_uint8_base2(uint8_t n); +void print_uint8_base10(uint8_t n); + void printFloat(float n); #endif \ No newline at end of file diff --git a/probe.c b/probe.c index 20b43a8..cad0449 100644 --- a/probe.c +++ b/probe.c @@ -30,13 +30,19 @@ void probe_init() } +// Returns the probe pin state. Triggered = true. Called by gcode parser and probe state monitor. +uint8_t probe_get_state() +{ + return(!(PROBE_PIN & PROBE_MASK)); +} + // Monitors probe pin state and records the system position when detected. Called by the // stepper ISR per ISR tick. // NOTE: This function must be extremely efficient as to not bog down the stepper ISR. void probe_state_monitor() { if (sys.probe_state == PROBE_ACTIVE) { - if (!(PROBE_PIN & PROBE_MASK)) { + if (probe_get_state()) { sys.probe_state = PROBE_OFF; memcpy(sys.probe_position, sys.position, sizeof(float)*N_AXIS); sys.execute |= EXEC_FEED_HOLD; diff --git a/probe.h b/probe.h index e5aef86..294c01b 100644 --- a/probe.h +++ b/probe.h @@ -29,6 +29,9 @@ // Probe pin initialization routine. void probe_init(); +// Returns probe pin state. +uint8_t probe_get_state(); + // Monitors probe pin state and records the system position when detected. Called by the // stepper ISR per ISR tick. void probe_state_monitor(); diff --git a/protocol.c b/protocol.c index 2bf7bcd..c97829b 100644 --- a/protocol.c +++ b/protocol.c @@ -36,37 +36,27 @@ static char line[LINE_BUFFER_SIZE]; // Line to be executed. Zero-terminated. // Directs and executes one line of formatted input from protocol_process. While mostly // incoming streaming g-code blocks, this also directs and executes Grbl internal commands, // such as settings, initiating the homing cycle, and toggling switch states. -// TODO: Eventually re-organize this function to more cleanly organize order of operations, -// which will hopefully reduce some of the current spaghetti logic and dynamic memory usage. static void protocol_execute_line(char *line) { protocol_execute_runtime(); // Runtime command check point. if (sys.abort) { return; } // Bail to calling function upon system abort - uint8_t status; if (line[0] == 0) { // Empty or comment line. Send status message for syncing purposes. - status = STATUS_OK; + report_status_message(STATUS_OK); } else if (line[0] == '$') { // Grbl '$' system command - status = system_execute_line(line); + report_status_message(system_execute_line(line)); + } else if (sys.state == STATE_ALARM) { + // Everything else is gcode. Block if in alarm mode. + report_status_message(STATUS_ALARM_LOCK); + } else { - // Everything else is gcode. Send to g-code parser! Block if in alarm mode. - if (sys.state == STATE_ALARM) { status = STATUS_ALARM_LOCK; } - else { status = gc_execute_line(line); } - - // TODO: Separate the parsing from the g-code execution. Need to re-write the parser - // completely to do this. First parse the line completely, checking for modal group - // errors and storing all of the g-code words. Then, send the stored g-code words to - // a separate g-code executor. This will be more in-line with actual g-code protocol. - - // TODO: Clean up the multi-tasking workflow with the execution of commands. It's a - // bit complicated and patch-worked. Could be made simplier to understand. + // Parse and execute g-code block! + report_status_message(gc_execute_line(line)); } - - report_status_message(status); } @@ -117,12 +107,27 @@ void protocol_main_loop() } } else { if (c <= ' ') { - // Throw away whitepace and control characters + // Throw away whitepace and control characters } else if (c == '/') { - // Block delete not supported. Ignore character. + // Block delete NOT SUPPORTED. Ignore character. + // NOTE: If supported, would simply need to check the system if block delete is enabled. } else if (c == '(') { // Enable comments flag and ignore all characters until ')' or EOL. + // NOTE: This doesn't follow the NIST definition exactly, but is good enough for now. + // In the future, we could simply remove the items within the comments, but retain the + // comment control characters, so that the g-code parser can error-check it. iscomment = true; + // } else if (c == ';') { + // Comment character to EOL NOT SUPPORTED. LinuxCNC definition. Not NIST. + + // TODO: Install '%' feature + // } else if (c == '%') { + // Program start-end percent sign NOT SUPPORTED. + // NOTE: This maybe installed to tell Grbl when a program is running vs manual input, + // where, during a program, the system auto-cycle start will continue to execute + // everything until the next '%' sign. This will help fix resuming issues with certain + // functions that empty the planner buffer to execute its task on-time. + } else if (char_counter >= LINE_BUFFER_SIZE-1) { // Detect line buffer overflow. Report error and reset line buffer. report_status_message(STATUS_OVERFLOW); diff --git a/report.c b/report.c index e766ee5..bed9661 100644 --- a/report.c +++ b/report.c @@ -51,22 +51,16 @@ void report_status_message(uint8_t status_code) } else { printPgmString(PSTR("error: ")); switch(status_code) { - case STATUS_BAD_NUMBER_FORMAT: - printPgmString(PSTR("Bad number format")); break; case STATUS_EXPECTED_COMMAND_LETTER: printPgmString(PSTR("Expected command letter")); break; - case STATUS_UNSUPPORTED_STATEMENT: - printPgmString(PSTR("Unsupported statement")); break; - case STATUS_ARC_RADIUS_ERROR: - printPgmString(PSTR("Invalid radius")); break; - case STATUS_MODAL_GROUP_VIOLATION: - printPgmString(PSTR("Modal group violation")); break; + case STATUS_BAD_NUMBER_FORMAT: + printPgmString(PSTR("Bad number format")); break; case STATUS_INVALID_STATEMENT: printPgmString(PSTR("Invalid statement")); break; + case STATUS_NEGATIVE_VALUE: + printPgmString(PSTR("Value < 0")); break; case STATUS_SETTING_DISABLED: printPgmString(PSTR("Setting disabled")); break; - case STATUS_SETTING_VALUE_NEG: - printPgmString(PSTR("Value < 0.0")); break; case STATUS_SETTING_STEP_PULSE_MIN: printPgmString(PSTR("Value < 3 usec")); break; case STATUS_SETTING_READ_FAIL: @@ -79,6 +73,18 @@ void report_status_message(uint8_t status_code) printPgmString(PSTR("Homing not enabled")); break; case STATUS_OVERFLOW: printPgmString(PSTR("Line overflow")); break; + + // Common g-code parser errors. + case STATUS_GCODE_MODAL_GROUP_VIOLATION: + printPgmString(PSTR("Modal group violation")); break; + case STATUS_GCODE_UNSUPPORTED_COMMAND: + printPgmString(PSTR("Unsupported command")); break; + case STATUS_GCODE_UNDEFINED_FEED_RATE: + printPgmString(PSTR("Undefined feed rate")); break; + default: + // Remaining g-code parser errors with error codes + printPgmString(PSTR("Invalid gcode ID:")); + print_uint8_base10(status_code); // Print error code for user reference } printPgmString(PSTR("\r\n")); } @@ -163,29 +169,28 @@ void report_grbl_settings() { printPgmString(PSTR(" (z accel, mm/sec^2)\r\n$9=")); printFloat(-settings.max_travel[X_AXIS]); // Grbl internally store this as negative. printPgmString(PSTR(" (x max travel, mm)\r\n$10=")); printFloat(-settings.max_travel[Y_AXIS]); // Grbl internally store this as negative. printPgmString(PSTR(" (y max travel, mm)\r\n$11=")); printFloat(-settings.max_travel[Z_AXIS]); // Grbl internally store this as negative. - printPgmString(PSTR(" (z max travel, mm)\r\n$12=")); printInteger(settings.pulse_microseconds); - printPgmString(PSTR(" (step pulse, usec)\r\n$13=")); printFloat(settings.default_feed_rate); - printPgmString(PSTR(" (default feed, mm/min)\r\n$14=")); printInteger(settings.step_invert_mask); + printPgmString(PSTR(" (z max travel, mm)\r\n$12=")); print_uint8_base10(settings.pulse_microseconds); + printPgmString(PSTR(" (step pulse, usec)\r\n$13=")); print_uint8_base10(settings.step_invert_mask); printPgmString(PSTR(" (step port invert mask:")); print_uint8_base2(settings.step_invert_mask); - printPgmString(PSTR(")\r\n$15=")); printInteger(settings.dir_invert_mask); + printPgmString(PSTR(")\r\n$14=")); print_uint8_base10(settings.dir_invert_mask); printPgmString(PSTR(" (dir port invert mask:")); print_uint8_base2(settings.dir_invert_mask); - printPgmString(PSTR(")\r\n$16=")); printInteger(settings.stepper_idle_lock_time); - printPgmString(PSTR(" (step idle delay, msec)\r\n$17=")); printFloat(settings.junction_deviation); - printPgmString(PSTR(" (junction deviation, mm)\r\n$18=")); printFloat(settings.arc_tolerance); - printPgmString(PSTR(" (arc tolerance, mm)\r\n$19=")); printInteger(settings.decimal_places); - printPgmString(PSTR(" (n-decimals, int)\r\n$20=")); printInteger(bit_istrue(settings.flags,BITFLAG_REPORT_INCHES)); - printPgmString(PSTR(" (report inches, bool)\r\n$21=")); printInteger(bit_istrue(settings.flags,BITFLAG_AUTO_START)); - printPgmString(PSTR(" (auto start, bool)\r\n$22=")); printInteger(bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)); - printPgmString(PSTR(" (invert step enable, bool)\r\n$23=")); printInteger(bit_istrue(settings.flags,BITFLAG_INVERT_LIMIT_PINS)); - printPgmString(PSTR(" (invert limit pins, bool)\r\n$24=")); printInteger(bit_istrue(settings.flags,BITFLAG_SOFT_LIMIT_ENABLE)); - printPgmString(PSTR(" (soft limits, bool)\r\n$25=")); printInteger(bit_istrue(settings.flags,BITFLAG_HARD_LIMIT_ENABLE)); - printPgmString(PSTR(" (hard limits, bool)\r\n$26=")); printInteger(bit_istrue(settings.flags,BITFLAG_HOMING_ENABLE)); - printPgmString(PSTR(" (homing cycle, bool)\r\n$27=")); printInteger(settings.homing_dir_mask); + printPgmString(PSTR(")\r\n$15=")); print_uint8_base10(settings.stepper_idle_lock_time); + printPgmString(PSTR(" (step idle delay, msec)\r\n$16=")); printFloat(settings.junction_deviation); + printPgmString(PSTR(" (junction deviation, mm)\r\n$17=")); printFloat(settings.arc_tolerance); + printPgmString(PSTR(" (arc tolerance, mm)\r\n$18=")); print_uint8_base10(settings.decimal_places); + printPgmString(PSTR(" (n-decimals, int)\r\n$19=")); print_uint8_base10(bit_istrue(settings.flags,BITFLAG_REPORT_INCHES)); + printPgmString(PSTR(" (report inches, bool)\r\n$20=")); print_uint8_base10(bit_istrue(settings.flags,BITFLAG_AUTO_START)); + printPgmString(PSTR(" (auto start, bool)\r\n$21=")); print_uint8_base10(bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)); + printPgmString(PSTR(" (invert step enable, bool)\r\n$22=")); print_uint8_base10(bit_istrue(settings.flags,BITFLAG_INVERT_LIMIT_PINS)); + printPgmString(PSTR(" (invert limit pins, bool)\r\n$23=")); print_uint8_base10(bit_istrue(settings.flags,BITFLAG_SOFT_LIMIT_ENABLE)); + printPgmString(PSTR(" (soft limits, bool)\r\n$24=")); print_uint8_base10(bit_istrue(settings.flags,BITFLAG_HARD_LIMIT_ENABLE)); + printPgmString(PSTR(" (hard limits, bool)\r\n$25=")); print_uint8_base10(bit_istrue(settings.flags,BITFLAG_HOMING_ENABLE)); + printPgmString(PSTR(" (homing cycle, bool)\r\n$26=")); print_uint8_base10(settings.homing_dir_mask); printPgmString(PSTR(" (homing dir invert mask:")); print_uint8_base2(settings.homing_dir_mask); - printPgmString(PSTR(")\r\n$28=")); printFloat(settings.homing_feed_rate); - printPgmString(PSTR(" (homing feed, mm/min)\r\n$29=")); printFloat(settings.homing_seek_rate); - printPgmString(PSTR(" (homing seek, mm/min)\r\n$30=")); printInteger(settings.homing_debounce_delay); - printPgmString(PSTR(" (homing debounce, msec)\r\n$31=")); printFloat(settings.homing_pulloff); + printPgmString(PSTR(")\r\n$27=")); printFloat(settings.homing_feed_rate); + printPgmString(PSTR(" (homing feed, mm/min)\r\n$28=")); printFloat(settings.homing_seek_rate); + printPgmString(PSTR(" (homing seek, mm/min)\r\n$29=")); print_uint8_base10(settings.homing_debounce_delay); + printPgmString(PSTR(" (homing debounce, msec)\r\n$30=")); printFloat(settings.homing_pulloff); printPgmString(PSTR(" (homing pull-off, mm)\r\n")); } @@ -222,16 +227,11 @@ void report_ngc_parameters() } printPgmString(PSTR("[G")); switch (coord_select) { - case 0: printPgmString(PSTR("54:")); break; - case 1: printPgmString(PSTR("55:")); break; - case 2: printPgmString(PSTR("56:")); break; - case 3: printPgmString(PSTR("57:")); break; - case 4: printPgmString(PSTR("58:")); break; - case 5: printPgmString(PSTR("59:")); break; - case 6: printPgmString(PSTR("28:")); break; - case 7: printPgmString(PSTR("30:")); break; - // case 8: printPgmString(PSTR("92:")); break; // G92.2, G92.3 not supported. Hence not stored. - } + case 6: printPgmString(PSTR("28")); break; + case 7: printPgmString(PSTR("30")); break; + default: print_uint8_base10(coord_select+54); break; // G54-G59 + } + printPgmString(PSTR(":")); for (i=0; i Date: Thu, 29 May 2014 15:41:53 -0600 Subject: [PATCH 55/73] Fixed spindle/coolant/dwell state check. --- coolant_control.c | 2 +- motion_control.c | 2 +- settings.h | 2 +- spindle_control.c | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/coolant_control.c b/coolant_control.c index 9d66c50..60cbf2f 100644 --- a/coolant_control.c +++ b/coolant_control.c @@ -45,7 +45,7 @@ void coolant_stop() void coolant_run(uint8_t mode) { - if (sys.state != STATE_CHECK_MODE) { return; } + if (sys.state == STATE_CHECK_MODE) { return; } protocol_buffer_synchronize(); // Ensure coolant turns on when specified in program. if (mode == COOLANT_FLOOD_ENABLE) { diff --git a/motion_control.c b/motion_control.c index 321dfe9..adc4a73 100644 --- a/motion_control.c +++ b/motion_control.c @@ -214,7 +214,7 @@ void mc_arc(float *position, float *target, float *offset, float radius, float f // Execute dwell in seconds. void mc_dwell(float seconds) { - if (sys.state != STATE_CHECK_MODE) { return; } + if (sys.state == STATE_CHECK_MODE) { return; } uint16_t i = floor(1000/DWELL_TIME_STEP*seconds); protocol_buffer_synchronize(); diff --git a/settings.h b/settings.h index a22e358..0be1035 100644 --- a/settings.h +++ b/settings.h @@ -26,7 +26,7 @@ #define GRBL_VERSION "0.9e" -#define GRBL_VERSION_BUILD "20140525" +#define GRBL_VERSION_BUILD "20140529" // Version of the EEPROM data. Will be used to migrate existing data from older versions of Grbl // when firmware is upgraded. Always stored in byte 0 of eeprom diff --git a/spindle_control.c b/spindle_control.c index b7dd36e..0ebfa85 100644 --- a/spindle_control.c +++ b/spindle_control.c @@ -57,7 +57,7 @@ void spindle_stop() void spindle_run(uint8_t direction, float rpm) { - if (sys.state != STATE_CHECK_MODE) { return; } + if (sys.state == STATE_CHECK_MODE) { return; } // Empty planner buffer to ensure spindle is set when programmed. protocol_buffer_synchronize(); From 8ed8005f6cc11d998b0a3546fc6d64e978d8277b Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Sat, 31 May 2014 21:58:59 -0600 Subject: [PATCH 56/73] Various minor g-code parser fixes. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Updated the mantissa calculation that checks for non-integer values and GXX.X commands that aren’t supported. There was a potential uint8 overflow issue. - Fixed g-code parser bug related to not using the correct modal struct. G10 P0 not selecting the current coordinate system when a G55-59 is issued in the same line. - Fixed g-code parser bug related to not using the correct modal struct. Target position locations were not computed correctly when G90/91 distance modes were changed in the same line. It was using the previous state, rather than the current block. --- gcode.c | 31 ++++++++++++++++++------------- 1 file changed, 18 insertions(+), 13 deletions(-) diff --git a/gcode.c b/gcode.c index e871121..ab9ad1f 100644 --- a/gcode.c +++ b/gcode.c @@ -124,9 +124,14 @@ uint8_t gc_execute_line(char *line) if (!read_float(line, &char_counter, &value)) { FAIL(STATUS_BAD_NUMBER_FORMAT); } // [Expected word value] // Convert values to smaller uint8 significand and mantissa values for parsing this word. - // NOTE: Mantissa is multiplied by 1000 to catch non-integer command values. + // NOTE: Mantissa is multiplied by 100 to catch non-integer command values. This is more + // accurate than the NIST gcode requirement of x10 when used for commands, but not quite + // accurate enough for value words that require integers to within 0.0001. This should be + // a good enough comprimise and catch most all non-integer errors. To make it compliant, + // we would simply need to change the mantissa to int16, but this add compiled flash space. + // Maybe update this later. int_value = trunc(value); - mantissa = trunc(1000*(value - int_value)); // Compute mantissa for Gxx.x commands + mantissa = trunc(100*(value - int_value)); // Compute mantissa for Gxx.x commands // Check if the g-code word is supported or errors due to modal group violations or has // been repeated in the g-code block. If ok, update the command or record its value. @@ -153,7 +158,7 @@ uint8_t gc_execute_line(char *line) case 28: switch(mantissa) { case 0: gc_block.non_modal_command = NON_MODAL_GO_HOME_0; break; // G28 - case 100: gc_block.non_modal_command = NON_MODAL_SET_HOME_0; break; // G28.1 + case 10: gc_block.non_modal_command = NON_MODAL_SET_HOME_0; break; // G28.1 default: FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); // [Unsupported G28.x command] } mantissa = 0; // Set to zero to indicate valid non-integer G command. @@ -161,7 +166,7 @@ uint8_t gc_execute_line(char *line) case 30: switch(mantissa) { case 0: gc_block.non_modal_command = NON_MODAL_GO_HOME_1; break; // G30 - case 100: gc_block.non_modal_command = NON_MODAL_SET_HOME_1; break; // G30.1 + case 10: gc_block.non_modal_command = NON_MODAL_SET_HOME_1; break; // G30.1 default: FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); // [Unsupported G30.x command] } mantissa = 0; // Set to zero to indicate valid non-integer G command. @@ -170,7 +175,7 @@ uint8_t gc_execute_line(char *line) case 92: switch(mantissa) { case 0: gc_block.non_modal_command = NON_MODAL_SET_COORDINATE_OFFSET; break; // G92 - case 100: gc_block.non_modal_command = NON_MODAL_RESET_COORDINATE_OFFSET; break; // G92.1 + case 10: gc_block.non_modal_command = NON_MODAL_RESET_COORDINATE_OFFSET; break; // G92.1 default: FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); // [Unsupported G92.x command] } mantissa = 0; // Set to zero to indicate valid non-integer G command. @@ -191,11 +196,11 @@ uint8_t gc_execute_line(char *line) case 3: gc_block.modal.motion = MOTION_MODE_CCW_ARC; break; // G3 case 38: switch(mantissa) { - case 200: gc_block.modal.motion = MOTION_MODE_PROBE; break; // G38.2 + case 20: gc_block.modal.motion = MOTION_MODE_PROBE; break; // G38.2 // NOTE: If G38.3+ are enabled, change mantissa variable type to uint16_t. - // case 300: gc_block.modal.motion = MOTION_MODE_PROBE_NO_ERROR; break; // G38.3 Not supported. - // case 400: // Not supported. - // case 500: // Not supported. + // case 30: gc_block.modal.motion = MOTION_MODE_PROBE_NO_ERROR; break; // G38.3 Not supported. + // case 40: // Not supported. + // case 50: // Not supported. default: FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); // [Unsupported G38.x command] } mantissa = 0; // Set to zero to indicate valid non-integer G command. @@ -324,6 +329,7 @@ uint8_t gc_execute_line(char *line) } } + // Parsing complete! /* ------------------------------------------------------------------------------------- @@ -338,8 +344,7 @@ uint8_t gc_execute_line(char *line) accurately calculated if we convert these values in conjunction with the error-checking. This relegates the next execution step as only updating the system g-code modes and performing the programmed actions in order. The execution step should not require any - conversion calculations and would only require minimal computations necessary to execute - the commands. + conversion calculations and would only require minimal checks necessary to execute. */ /* NOTE: At this point, the g-code block has been parsed and the block line can be freed. @@ -510,7 +515,7 @@ uint8_t gc_execute_line(char *line) // Load EEPROM coordinate data and pre-calculate the new coordinate data. if (int_value > 0) { int_value--; } // Adjust P1-P6 index to EEPROM coordinate data indexing. - else { int_value = gc_state.modal.coord_select; } // Index P0 as the active coordinate system + else { int_value = gc_block.modal.coord_select; } // Index P0 as the active coordinate system if (!settings_read_coord_data(int_value,parameter_data)) { FAIL(STATUS_SETTING_READ_FAIL); } // [EEPROM read fail] for (idx=0; idx Date: Sat, 31 May 2014 23:23:21 -0600 Subject: [PATCH 57/73] Arc error-checking update. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Updated offset-mode arc error-checking to EMC2’s version: The old NIST definition required the radii to the current location and target location to differ no more than 0.002mm. This proved to be problematic and probably why LinuxCNC(EMC2) updated it to be 0.005mm AND 0.1% radius OR 0.5mm. --- gcode.c | 8 ++++++-- settings.h | 2 +- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/gcode.c b/gcode.c index ab9ad1f..5214fc4 100644 --- a/gcode.c +++ b/gcode.c @@ -742,8 +742,12 @@ uint8_t gc_execute_line(char *line) y -= gc_block.values.ijk[axis_1]; // Delta y between circle center and target float target_r = hypot_f(x,y); gc_block.values.r = hypot_f(gc_block.values.ijk[axis_0], gc_block.values.ijk[axis_1]); // Compute arc radius for mc_arc - - if (fabs(target_r-gc_block.values.r) > 0.002) { FAIL(STATUS_GCODE_INVALID_TARGET); } // [Arc definition error] + + target_r = fabs(target_r-gc_block.values.r); + if (target_r > 0.005) { + if (target_r > 0.5) { FAIL(STATUS_GCODE_INVALID_TARGET); } // [Arc definition error] + if (target_r > 0.001*gc_block.values.r) { FAIL(STATUS_GCODE_INVALID_TARGET); } // [Arc definition error] + } } break; case MOTION_MODE_PROBE: diff --git a/settings.h b/settings.h index 0be1035..12bbba3 100644 --- a/settings.h +++ b/settings.h @@ -26,7 +26,7 @@ #define GRBL_VERSION "0.9e" -#define GRBL_VERSION_BUILD "20140529" +#define GRBL_VERSION_BUILD "20140531" // Version of the EEPROM data. Will be used to migrate existing data from older versions of Grbl // when firmware is upgraded. Always stored in byte 0 of eeprom From 015d5fa19105f90694ca46986fa264893bc7155a Mon Sep 17 00:00:00 2001 From: Krzysztof Foltman Date: Thu, 26 Jun 2014 23:06:26 +0100 Subject: [PATCH 58/73] Fixed atomic access to flags in sys.execute. This seems to fix the bug that caused Grbl to hang during some operations, especially jogging. --- limits.c | 10 +++++----- motion_control.c | 10 +++++----- nuts_bolts.h | 6 +++--- probe.c | 2 +- protocol.c | 2 +- serial.c | 6 +++--- system.c | 4 ++-- 7 files changed, 20 insertions(+), 20 deletions(-) diff --git a/limits.c b/limits.c index c31988a..f6e5f01 100644 --- a/limits.c +++ b/limits.c @@ -86,7 +86,7 @@ ISR(LIMIT_INT_vect) // DEFAULT: Limit pin change interrupt process. if (sys.state != STATE_ALARM) { if (bit_isfalse(sys.execute,EXEC_ALARM)) { mc_reset(); // Initiate system kill. - sys.execute |= (EXEC_ALARM | EXEC_CRIT_EVENT); // Indicate hard limit critical event + bit_true(sys.execute, (EXEC_ALARM | EXEC_CRIT_EVENT)); // Indicate hard limit critical event } } } @@ -103,7 +103,7 @@ ISR(WDT_vect) // Watchdog timer ISR if (bit_istrue(settings.flags,BITFLAG_INVERT_LIMIT_PINS)) { bits ^= LIMIT_MASK; } if (bits & LIMIT_MASK) { mc_reset(); // Initiate system kill. - sys.execute |= (EXEC_ALARM | EXEC_CRIT_EVENT); // Indicate hard limit critical event + bit_true(sys.execute, (EXEC_ALARM | EXEC_CRIT_EVENT)); // Indicate hard limit critical event } } } @@ -238,7 +238,7 @@ void limits_go_home(uint8_t cycle_mask) // Initiate pull-off using main motion control routines. // TODO : Clean up state routines so that this motion still shows homing state. sys.state = STATE_QUEUED; - sys.execute |= EXEC_CYCLE_START; + bit_true(sys.execute, EXEC_CYCLE_START); protocol_execute_runtime(); protocol_buffer_synchronize(); // Complete pull-off motion. @@ -259,7 +259,7 @@ void limits_soft_check(float *target) // workspace volume so just come to a controlled stop so position is not lost. When complete // enter alarm mode. if (sys.state == STATE_CYCLE) { - sys.execute |= EXEC_FEED_HOLD; + bit_true(sys.execute, EXEC_FEED_HOLD); do { protocol_execute_runtime(); if (sys.abort) { return; } @@ -267,7 +267,7 @@ void limits_soft_check(float *target) } mc_reset(); // Issue system reset and ensure spindle and coolant are shutdown. - sys.execute |= (EXEC_ALARM | EXEC_CRIT_EVENT); // Indicate soft limit critical event + bit_true(sys.execute, (EXEC_ALARM | EXEC_CRIT_EVENT)); // Indicate soft limit critical event protocol_execute_runtime(); // Execute to enter critical event loop and system abort return; diff --git a/motion_control.c b/motion_control.c index adc4a73..c57dfea 100644 --- a/motion_control.c +++ b/motion_control.c @@ -288,13 +288,13 @@ void mc_probe_cycle(float *target, float feed_rate, uint8_t invert_feed_rate) // NOTE: Parser error-checking ensures the probe isn't already closed/triggered. sys.probe_state = PROBE_ACTIVE; - sys.execute |= EXEC_CYCLE_START; + bit_true(sys.execute, EXEC_CYCLE_START); do { protocol_execute_runtime(); if (sys.abort) { return; } // Check for system abort } while ((sys.state != STATE_IDLE) && (sys.state != STATE_QUEUED)); - if (sys.probe_state == PROBE_ACTIVE) { sys.execute |= EXEC_CRIT_EVENT; } + if (sys.probe_state == PROBE_ACTIVE) { bit_true(sys.execute, EXEC_CRIT_EVENT); } protocol_execute_runtime(); // Check and execute run-time commands if (sys.abort) { return; } // Check for system abort @@ -316,7 +316,7 @@ void mc_probe_cycle(float *target, float feed_rate, uint8_t invert_feed_rate) mc_line(target, feed_rate, invert_feed_rate); // Bypass mc_line(). Directly plan homing motion. #endif - sys.execute |= EXEC_CYCLE_START; + bit_true(sys.execute, EXEC_CYCLE_START); protocol_buffer_synchronize(); // Complete pull-off motion. if (sys.abort) { return; } // Did not complete. Alarm state set by mc_alarm. @@ -337,7 +337,7 @@ void mc_reset() { // Only this function can set the system reset. Helps prevent multiple kill calls. if (bit_isfalse(sys.execute, EXEC_RESET)) { - sys.execute |= EXEC_RESET; + bit_true(sys.execute, EXEC_RESET); // Kill spindle and coolant. spindle_stop(); @@ -348,7 +348,7 @@ void mc_reset() // the steppers enabled by avoiding the go_idle call altogether, unless the motion state is // violated, by which, all bets are off. if (sys.state & (STATE_CYCLE | STATE_HOLD | STATE_HOMING)) { - sys.execute |= EXEC_ALARM; // Flag main program to execute alarm state. + bit_true(sys.execute, EXEC_ALARM); // Flag main program to execute alarm state. st_go_idle(); // Force kill steppers. Position has likely been lost. } } diff --git a/nuts_bolts.h b/nuts_bolts.h index d68af30..5cab6f4 100644 --- a/nuts_bolts.h +++ b/nuts_bolts.h @@ -44,9 +44,9 @@ // Bit field and masking macros #define bit(n) (1 << n) -#define bit_true(x,mask) (x |= mask) -#define bit_false(x,mask) (x &= ~mask) -#define bit_toggle(x,mask) (x ^= mask) +#define bit_true(x,mask) {uint8_t sreg = SREG; cli(); (x) |= (mask); SREG = sreg; } +#define bit_false(x,mask) {uint8_t sreg = SREG; cli(); (x) &= ~(mask); SREG = sreg; } +#define bit_toggle(x,mask) {uint8_t sreg = SREG; cli(); (x) ^= (mask); SREG = sreg; } #define bit_istrue(x,mask) ((x & mask) != 0) #define bit_isfalse(x,mask) ((x & mask) == 0) diff --git a/probe.c b/probe.c index cad0449..84ec01d 100644 --- a/probe.c +++ b/probe.c @@ -45,7 +45,7 @@ void probe_state_monitor() if (probe_get_state()) { sys.probe_state = PROBE_OFF; memcpy(sys.probe_position, sys.position, sizeof(float)*N_AXIS); - sys.execute |= EXEC_FEED_HOLD; + bit_true(sys.execute, EXEC_FEED_HOLD); } } } diff --git a/protocol.c b/protocol.c index c97829b..3f22489 100644 --- a/protocol.c +++ b/protocol.c @@ -288,4 +288,4 @@ void protocol_buffer_synchronize() // NOTE: This function is called from the main loop and mc_line() only and executes when one of // two conditions exist respectively: There are no more blocks sent (i.e. streaming is finished, // single commands), or the planner buffer is full and ready to go. -void protocol_auto_cycle_start() { if (sys.auto_start) { sys.execute |= EXEC_CYCLE_START; } } +void protocol_auto_cycle_start() { if (sys.auto_start) { bit_true(sys.execute, EXEC_CYCLE_START); } } diff --git a/serial.c b/serial.c index 29e361c..3b1a8d5 100644 --- a/serial.c +++ b/serial.c @@ -157,9 +157,9 @@ ISR(SERIAL_RX) // Pick off runtime command characters directly from the serial stream. These characters are // not passed into the buffer, but these set system state flag bits for runtime execution. switch (data) { - case CMD_STATUS_REPORT: sys.execute |= EXEC_STATUS_REPORT; break; // Set as true - case CMD_CYCLE_START: sys.execute |= EXEC_CYCLE_START; break; // Set as true - case CMD_FEED_HOLD: sys.execute |= EXEC_FEED_HOLD; break; // Set as true + case CMD_STATUS_REPORT: bit_true(sys.execute, EXEC_STATUS_REPORT); break; // Set as true + case CMD_CYCLE_START: bit_true(sys.execute, EXEC_CYCLE_START); break; // Set as true + case CMD_FEED_HOLD: bit_true(sys.execute, EXEC_FEED_HOLD); break; // Set as true case CMD_RESET: mc_reset(); break; // Call motion control reset routine. default: // Write character to buffer next_head = rx_buffer_head + 1; diff --git a/system.c b/system.c index 181a07b..83177d2 100644 --- a/system.c +++ b/system.c @@ -46,9 +46,9 @@ ISR(PINOUT_INT_vect) if (bit_isfalse(PINOUT_PIN,bit(PIN_RESET))) { mc_reset(); } else if (bit_isfalse(PINOUT_PIN,bit(PIN_FEED_HOLD))) { - sys.execute |= EXEC_FEED_HOLD; + bit_true(sys.execute, EXEC_FEED_HOLD); } else if (bit_isfalse(PINOUT_PIN,bit(PIN_CYCLE_START))) { - sys.execute |= EXEC_CYCLE_START; + bit_true(sys.execute, EXEC_CYCLE_START); } } } From 1ef5a4547900c94f89d5958f403094340b154cfd Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Wed, 2 Jul 2014 08:39:19 -0600 Subject: [PATCH 59/73] Minor bug fixes and updates. Line number tracking. - Line number tracking was getting truncated at 255, since it was using wrong variable type. Fixed it with a trunc(). - Increased the max number line allowed by Grbl to 9999999 from the g-code standard 99999. The latter seems to be an arbitrary number, so we are allowing larger ones for at least one known use case scenario. - Created a new test directory to contain some testing g-code to proof the firmware. Only got started with one test case so far. More will be inserted as needed. - Some other commenting updates to clarify certain aspects of the code. --- gcode.c | 29 ++++++++++++++++++----------- nuts_bolts.c | 1 + protocol.c | 16 ++++++++++++---- test/test.py | 25 +++++++++++++++++++++++++ 4 files changed, 56 insertions(+), 15 deletions(-) create mode 100644 test/test.py diff --git a/gcode.c b/gcode.c index 5214fc4..a4a8e70 100644 --- a/gcode.c +++ b/gcode.c @@ -29,7 +29,10 @@ #include "probe.h" #include "report.h" -#define MAX_LINE_NUMBER 99999 +// NOTE: Max line number is defined by the g-code standard to be 99999. It seems to be an +// arbitrary value, and some GUIs may require more. So we increased it based on a max safe +// value when converting a float (7.2 digit precision)s to an integer. +#define MAX_LINE_NUMBER 9999999 #define AXIS_COMMAND_NONE 0 #define AXIS_COMMAND_NON_MODAL 1 @@ -91,8 +94,8 @@ uint8_t gc_execute_line(char *line) memcpy(&gc_block.modal,&gc_state.modal,sizeof(gc_modal_t)); // Copy current modes uint8_t axis_explicit = AXIS_COMMAND_NONE; uint8_t axis_0, axis_1, axis_linear; - float coordinate_data[N_AXIS]; - float parameter_data[N_AXIS]; + float coordinate_data[N_AXIS]; // Multi-use variable to store coordinate data for execution + float parameter_data[N_AXIS]; // Multi-use variable to store parameter data for execution // Initialize bitflag tracking variables for axis indices compatible operations. uint8_t axis_words = 0; // XYZ tracking @@ -305,7 +308,7 @@ uint8_t gc_execute_line(char *line) case 'J': word_bit = WORD_J; gc_block.values.ijk[Y_AXIS] = value; ijk_words |= (1< 0.005) { - if (target_r > 0.5) { FAIL(STATUS_GCODE_INVALID_TARGET); } // [Arc definition error] - if (target_r > 0.001*gc_block.values.r) { FAIL(STATUS_GCODE_INVALID_TARGET); } // [Arc definition error] + // Compute difference between current location and target radii for final error-checks. + float delta_r = fabs(target_r-gc_block.values.r); + if (delta_r > 0.005) { + if (delta_r > 0.5) { FAIL(STATUS_GCODE_INVALID_TARGET); } // [Arc definition error] > 0.5mm + if (delta_r > (0.001*gc_block.values.r)) { FAIL(STATUS_GCODE_INVALID_TARGET); } // [Arc definition error] > 0.005mm AND 0.1% radius } } break; diff --git a/nuts_bolts.c b/nuts_bolts.c index dff34d4..a8a159c 100644 --- a/nuts_bolts.c +++ b/nuts_bolts.c @@ -151,6 +151,7 @@ uint8_t get_direction_mask(uint8_t axis_idx) return(axis_mask); } + float hypot_f(float x, float y) { return(sqrt(x*x + y*y)); diff --git a/protocol.c b/protocol.c index c97829b..788115b 100644 --- a/protocol.c +++ b/protocol.c @@ -61,7 +61,7 @@ static void protocol_execute_line(char *line) /* - GRBL MAIN LOOP: + GRBL PRIMARY LOOP: */ void protocol_main_loop() { @@ -81,9 +81,9 @@ void protocol_main_loop() system_execute_startup(line); // Execute startup script. } - // ------------------------------------------------------------------------------ - // Main loop! Upon a system abort, this exits back to main() to reset the system. - // ------------------------------------------------------------------------------ + // --------------------------------------------------------------------------------- + // Primary loop! Upon a system abort, this exits back to main() to reset the system. + // --------------------------------------------------------------------------------- uint8_t iscomment = false; uint8_t char_counter = 0; @@ -92,6 +92,14 @@ void protocol_main_loop() // Process one line of incoming serial data, as the data becomes available. Performs an // initial filtering by removing spaces and comments and capitalizing all letters. + + // NOTE: While comment, spaces, and block delete(if supported) handling should technically + // be done in the g-code parser, doing it here helps compress the incoming data into Grbl's + // line buffer, which is limited in size. The g-code standard actually states a line can't + // exceed 256 characters, but the Arduino Uno does not have the memory space for this. + // With a better processor, it would be very easy to pull this initial parsing out as a + // seperate task to be shared by the g-code parser and Grbl's system commands. + while((c = serial_read()) != SERIAL_NO_DATA) { if ((c == '\n') || (c == '\r')) { // End of line reached line[char_counter] = 0; // Set string termination character. diff --git a/test/test.py b/test/test.py new file mode 100644 index 0000000..41c845c --- /dev/null +++ b/test/test.py @@ -0,0 +1,25 @@ +import random +import serial +import time +ser = serial.Serial('/dev/tty.usbmodem24111', 115200, timeout=0.001) +time.sleep(1) +outstanding = 0 +data = '' +while True: + time.sleep(0.1) + data += ser.read() + pos = data.find('\n') + if pos == -1: + line = '' + else: + line = data[0:pos + 1] + data = data[pos + 1:] + if line == '' and outstanding < 3: + while outstanding < 3: + ser.write("G0 Z%0.3f\n" % (0.01 * (random.random() - 0.5))) + #ser.write("M3\n") + outstanding += 1 + continue + if line == 'ok\r\n': + outstanding -= 1 + print outstanding, repr(line.rstrip()) \ No newline at end of file From 92d6c2bca51e4fce0e0fb7cc7239a219e5b56141 Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Thu, 3 Jul 2014 18:13:39 -0600 Subject: [PATCH 60/73] G-code parser G0/G1 bug fix. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Although stated as invalid in the NIST g-code standard, most g-code parsers, including linuxcnc, allow G0 and G1 to be commanded without axis words present. For example, something like ‘G1 F100.0’ to preset the motion mode and feed rate without a motion commanded. Older CNC controllers actually required this for feed rate settings. This update should now allow this type of behavior. --- gcode.c | 70 ++++++++++++++++++++++++++++----------------------------- 1 file changed, 34 insertions(+), 36 deletions(-) diff --git a/gcode.c b/gcode.c index a4a8e70..8557956 100644 --- a/gcode.c +++ b/gcode.c @@ -92,7 +92,7 @@ uint8_t gc_execute_line(char *line) memset(&gc_block, 0, sizeof(gc_block)); // Initialize the parser block struct. memcpy(&gc_block.modal,&gc_state.modal,sizeof(gc_modal_t)); // Copy current modes - uint8_t axis_explicit = AXIS_COMMAND_NONE; + uint8_t axis_command = AXIS_COMMAND_NONE; uint8_t axis_0, axis_1, axis_linear; float coordinate_data[N_AXIS]; // Multi-use variable to store coordinate data for execution float parameter_data[N_AXIS]; // Multi-use variable to store parameter data for execution @@ -149,8 +149,8 @@ uint8_t gc_execute_line(char *line) case 10: case 28: case 30: case 92: // Check for G10/28/30/92 being called with G0/1/2/3/38 on same block. if (mantissa == 0) { // Ignore G28.1, G30.1, and G92.1 - if (axis_explicit) { FAIL(STATUS_GCODE_AXIS_COMMAND_CONFLICT); } // [Axis word/command conflict] - axis_explicit = AXIS_COMMAND_NON_MODAL; + if (axis_command) { FAIL(STATUS_GCODE_AXIS_COMMAND_CONFLICT); } // [Axis word/command conflict] + axis_command = AXIS_COMMAND_NON_MODAL; } // No break. Continues to next line. case 4: case 53: @@ -187,8 +187,8 @@ uint8_t gc_execute_line(char *line) break; case 0: case 1: case 2: case 3: case 38: // Check for G0/1/2/3/38 being called with G10/28/30/92 on same block. - if (axis_explicit) { FAIL(STATUS_GCODE_AXIS_COMMAND_CONFLICT); } // [Axis word/command conflict] - axis_explicit = AXIS_COMMAND_MOTION_MODE; + if (axis_command) { FAIL(STATUS_GCODE_AXIS_COMMAND_CONFLICT); } // [Axis word/command conflict] + axis_command = AXIS_COMMAND_MOTION_MODE; // No break. Continues to next line. case 80: word_bit = MODAL_GROUP_G1; @@ -363,21 +363,10 @@ uint8_t gc_execute_line(char *line) // [0. Non-specific/common error-checks and miscellaneous setup]: - // Determine how axis words are used and error-check it. Apply implicit conditions and exceptions. - if (axis_explicit) { // Either a non-modal or motion mode axis-explicit command has been passed. - // Axis-words are required (with exceptions) with axis-explicit commands. - // NOTE: G28/30 reserve any axis words as an intermediate motion, but are not required. G2/3 full - // circle arcs don't require axis words, but this aren't supported. - if (!axis_words) { - if ((gc_block.non_modal_command == NON_MODAL_GO_HOME_0) || (gc_block.non_modal_command == NON_MODAL_GO_HOME_1)) { - axis_explicit = AXIS_COMMAND_NONE; // No axis words passed for G28/30. - } else { - FAIL(STATUS_GCODE_NO_AXIS_WORDS); // [No axis words] - } - } - } else { - // If no axis-explicit commands in the block, axis words implicitly activate the current motion mode. - if (axis_words) { axis_explicit = AXIS_COMMAND_MOTION_MODE; } // Assign implicit motion-mode + // Determine implicit axis command conditions. Axis words have been passed, but no explicit axis + // command has been sent. If so, set axis command to current motion mode. + if (axis_words) { + if (!axis_command) { axis_command = AXIS_COMMAND_MOTION_MODE; } // Assign implicit motion-mode } // Check for valid line number N value. @@ -391,7 +380,7 @@ uint8_t gc_execute_line(char *line) // NOTE: Single-meaning value words are removed all at once at the end of error-checking, because // they are always used when present. This was done to save a few bytes of flash. For clarity, the // single-meaning value words may be removed as they are used. Also, axis words are treated in the - // same way. If there is an axis explicit/implicit command, XYZ words are always used and are + // same way. If there is an explicit/implicit axis command, XYZ words are always used and are // are removed at the end of error-checking. // [1. Comments ]: MSG's NOT SUPPORTED. Comment handling performed by protocol. @@ -400,7 +389,7 @@ uint8_t gc_execute_line(char *line) // is not defined after switching to G94 from G93. if (gc_block.modal.feed_rate == FEED_RATE_MODE_INVERSE_TIME) { // = G93 // NOTE: G38 can also operate in inverse time, but is undefined as an error. Missing F word check added here. - if (axis_explicit == AXIS_COMMAND_MOTION_MODE) { + if (axis_command == AXIS_COMMAND_MOTION_MODE) { if ((gc_block.modal.motion != MOTION_MODE_NONE) || (gc_block.modal.motion != MOTION_MODE_SEEK)) { if (bit_isfalse(value_words,bit(WORD_F))) { FAIL(STATUS_GCODE_UNDEFINED_FEED_RATE); } // [F word missing] } @@ -497,15 +486,16 @@ uint8_t gc_execute_line(char *line) // [18. Set retract mode ]: NOT SUPPORTED. // [19. Remaining non-modal actions ]: Check go to predefined position, set G10, or set axis offsets. - // NOTE: We need to separate the non-modal commands that are axis-explicit (G10/G28/G30/G92), as these + // NOTE: We need to separate the non-modal commands that are axis word-using (G10/G28/G30/G92), as these // commands all treat axis words differently. G10 as absolute offsets or computes current position as // the axis value, G92 similarly to G10 L20, and G28/30 as an intermediate target position that observes // all the current coordinate system and G92 offsets. switch (gc_block.non_modal_command) { case NON_MODAL_SET_COORDINATE_DATA: // [G10 Errors]: L missing and is not 2 or 20. P word missing. (Negative P value done.) - // [G10 L2 Errors]: R word NOT SUPPORTED. P value not 0 to nCoordSys(max 9). Axis words missing (done.) - // [G10 L20 Errors]: P must be 0 to nCoordSys(max 9). Axis words missing (done.) + // [G10 L2 Errors]: R word NOT SUPPORTED. P value not 0 to nCoordSys(max 9). Axis words missing. + // [G10 L20 Errors]: P must be 0 to nCoordSys(max 9). Axis words missing. + if (!axis_words) { FAIL(STATUS_GCODE_NO_AXIS_WORDS) }; // [No axis words] if (bit_isfalse(value_words,((1< N_COORDINATE_SYSTEM) { FAIL(STATUS_GCODE_UNSUPPORTED_COORD_SYS); } // [Greater than N sys] @@ -533,7 +523,8 @@ uint8_t gc_execute_line(char *line) } break; case NON_MODAL_SET_COORDINATE_OFFSET: - // [G92 Errors]: No axis words (done.) + // [G92 Errors]: No axis words. + if (!axis_words) { FAIL(STATUS_GCODE_NO_AXIS_WORDS); } // [No axis words] // Update axes defined only in block. Offsets current system to defined value. Does not update when // active coordinate system is selected, but is still active unless G92.1 disables it. @@ -548,7 +539,7 @@ uint8_t gc_execute_line(char *line) default: - // At this point, the rest of the axis-explicit commands treat the axis values as the traditional + // At this point, the rest of the explicit axis commands treat the axis values as the traditional // target position with the coordinate system offsets, G92 offsets, absolute override, and distance // modes applied. This includes the motion mode commands. We can now pre-compute the target position. // NOTE: Tool offsets may be appended to these conversions when/if this feature is added. @@ -603,26 +594,31 @@ uint8_t gc_execute_line(char *line) // [20. Motion modes ]: if (gc_block.modal.motion == MOTION_MODE_NONE) { // [G80 Errors]: Axis word exist and are not used by a non-modal command. - if ((axis_words) && (axis_explicit != AXIS_COMMAND_NON_MODAL)) { + if ((axis_words) && (axis_command != AXIS_COMMAND_NON_MODAL)) { FAIL(STATUS_GCODE_AXIS_WORDS_EXIST); // [No axis words allowed] } // Check remaining motion modes, if axis word are implicit (exist and not used by G10/28/30/92), or // was explicitly commanded in the g-code block. - } else if ( axis_explicit == AXIS_COMMAND_MOTION_MODE ) { + } else if ( axis_command == AXIS_COMMAND_MOTION_MODE ) { - // [G0 Errors]: (All done!) No axis words. Axis letter not configured or without real value. + if (gc_block.modal.motion == MOTION_MODE_SEEK) { + // [G0 Errors]: Axis letter not configured or without real value (done.) + // Axis words are optional. If missing, set axis command flag to ignore execution. + if (!axis_words) { axis_command = AXIS_COMMAND_NONE; } // All remaining motion modes (all but G0 and G80), require a valid feed rate value. In units per mm mode, // the value must be positive. In inverse time mode, a positive value must be passed with each block. - if (gc_block.modal.motion != MOTION_MODE_SEEK) { - + } else { // Check if feed rate is defined for the motion modes that require it. if (gc_block.values.f == 0.0) { FAIL(STATUS_GCODE_UNDEFINED_FEED_RATE); } // [Feed rate undefined] switch (gc_block.modal.motion) { case MOTION_MODE_LINEAR: - // [G1 Errors]: (All done!) No axis words and feed rate undefined. Axis letter not configured or without real value. + // [G1 Errors]: Feed rate undefined. Axis letter not configured or without real value. + // Axis words are optional. If missing, set axis command flag to ignore execution. + if (!axis_words) { axis_command = AXIS_COMMAND_NONE; } + break; case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC: // [G2/3 Errors All-Modes]: Feed rate undefined. @@ -632,6 +628,7 @@ uint8_t gc_execute_line(char *line) // [G2/3 Full-Circle-Mode Errors]: NOT SUPPORTED. Axis words exist. No offsets programmed. P must be an integer. // NOTE: Both radius and offsets are required for arc tracing and are pre-computed with the error-checking. + if (!axis_words) { FAIL(STATUS_GCODE_NO_AXIS_WORDS); } // [No axis words] if (!(axis_words & (bit(axis_0)|bit(axis_1)))) { FAIL(STATUS_GCODE_NO_AXIS_WORDS_IN_PLANE); } // [No axis words in plane] // Calculate the change in position along each selected axis @@ -760,6 +757,7 @@ uint8_t gc_execute_line(char *line) case MOTION_MODE_PROBE: // [G38 Errors]: Target is same current. No axis words. Cutter compensation is enabled. Feed rate // is undefined. Probe is triggered. + if (!axis_words) { FAIL(STATUS_GCODE_NO_AXIS_WORDS); } // [No axis words] if (gc_check_same_position(gc_state.position, gc_block.values.xyz)) { FAIL(STATUS_GCODE_INVALID_TARGET); } // [Invalid target] if (probe_get_state()) { FAIL(STATUS_GCODE_PROBE_TRIGGERED); } // [Probe triggered] break; @@ -772,7 +770,7 @@ uint8_t gc_execute_line(char *line) // [0. Non-specific error-checks]: Complete unused value words check, i.e. IJK used when in arc // radius mode, or axis words that aren't used in the block. bit_false(value_words,(bit(WORD_N)|bit(WORD_F)|bit(WORD_S)|bit(WORD_T))); // Remove single-meaning value words. - if (axis_explicit) { bit_false(value_words,(bit(WORD_X)|bit(WORD_Y)|bit(WORD_Z))); } // Remove axis words. + if (axis_command) { bit_false(value_words,(bit(WORD_X)|bit(WORD_Y)|bit(WORD_Z))); } // Remove axis words. if (value_words) { FAIL(STATUS_GCODE_UNUSED_WORDS); } // [Unused words] @@ -859,7 +857,7 @@ uint8_t gc_execute_line(char *line) case NON_MODAL_GO_HOME_0: case NON_MODAL_GO_HOME_1: // Move to intermediate position before going home. Obeys current coordinate system and offsets // and absolute and incremental modes. - if (axis_explicit) { + if (axis_command) { #ifdef USE_LINE_NUMBERS mc_line(gc_block.values.xyz, -1.0, false, gc_block.values.n); #else @@ -893,7 +891,7 @@ uint8_t gc_execute_line(char *line) // Enter motion modes only if there are axis words or a motion mode command word in the block. gc_state.modal.motion = gc_block.modal.motion; if (gc_state.modal.motion != MOTION_MODE_NONE) { - if (axis_explicit == AXIS_COMMAND_MOTION_MODE) { + if (axis_command == AXIS_COMMAND_MOTION_MODE) { switch (gc_state.modal.motion) { case MOTION_MODE_SEEK: #ifdef USE_LINE_NUMBERS From 8c9f3bca659f53790f25008f88c3ae9d53a86d57 Mon Sep 17 00:00:00 2001 From: ashelly Date: Fri, 4 Jul 2014 11:14:54 -0400 Subject: [PATCH 61/73] Total rework of simulator for dev branch. Create separate thread for interrupt processes. Tick-accurate simulation of timers. Non-blocking character input for running in realtime mode. Decouple hardware sim from grbl code as much as possible. Expanded command line options. Provisions for cross-platform solution. --- .gitignore | 2 + config.h | 2 +- coolant_control.c | 3 +- gcode.c | 4 +- serial.c | 1 + sim/.gitignore | 1 + sim/Makefile | 21 ++- sim/avr/interrupt.c | 128 ++++++++++++-- sim/avr/interrupt.h | 71 +++----- sim/avr/io.c | 4 + sim/avr/io.h | 182 +++++++++++++++++++- sim/avr/wdt.h | 1 + sim/config.h | 91 +--------- sim/eeprom.c | 61 ++++++- sim/eeprom.h | 3 + sim/kbhit.c | 6 + sim/{runtime.c => kbhit.h} | 77 ++++----- sim/main.c | 148 ++++++++++++---- sim/planner_inject_accessors.c | 8 +- sim/platform.h | 23 +++ sim/platform_LINUX.c | 116 +++++++++++++ sim/platform_WINDOWS.c | 72 ++++++++ sim/platform_linux.h | 16 ++ sim/platform_windows.h | 16 ++ sim/rename_execute_runtime.h | 2 - sim/rename_main.h | 1 + sim/serial.c | 65 +++++--- sim/serial_hooks.h | 2 + sim/sim.sh | 2 + sim/simulator.c | 297 +++++++++++++++++---------------- sim/simulator.h | 56 +++++-- spindle_control.c | 1 + stepper.c | 16 +- 33 files changed, 1062 insertions(+), 437 deletions(-) create mode 100644 sim/avr/io.c create mode 100644 sim/avr/wdt.h create mode 100644 sim/eeprom.h create mode 100644 sim/kbhit.c rename sim/{runtime.c => kbhit.h} (50%) create mode 100644 sim/platform.h create mode 100644 sim/platform_LINUX.c create mode 100644 sim/platform_WINDOWS.c create mode 100644 sim/platform_linux.h create mode 100644 sim/platform_windows.h delete mode 100644 sim/rename_execute_runtime.h create mode 100644 sim/rename_main.h create mode 100644 sim/serial_hooks.h create mode 100755 sim/sim.sh diff --git a/.gitignore b/.gitignore index 844cd7a..326f9fc 100644 --- a/.gitignore +++ b/.gitignore @@ -3,3 +3,5 @@ *.elf *.DS_Store *.d +*~ +\#*\# diff --git a/config.h b/config.h index ccba270..6a1f77c 100644 --- a/config.h +++ b/config.h @@ -84,7 +84,7 @@ // parser state depending on user preferences. #define N_STARTUP_LINE 2 // Integer (1-3) -// Allows GRBL to tranck and report gcode line numbers. Enabling this means that the planning buffer +// Allows GRBL to track and report gcode line numbers. Enabling this means that the planning buffer // goes from 18 or 16 to make room for the additional line number data in the plan_block_t struct // #define USE_LINE_NUMBERS // Disabled by default. Uncomment to enable. diff --git a/coolant_control.c b/coolant_control.c index 60cbf2f..3c1a681 100644 --- a/coolant_control.c +++ b/coolant_control.c @@ -46,7 +46,8 @@ void coolant_stop() void coolant_run(uint8_t mode) { if (sys.state == STATE_CHECK_MODE) { return; } - + + protocol_auto_cycle_start(); protocol_buffer_synchronize(); // Ensure coolant turns on when specified in program. if (mode == COOLANT_FLOOD_ENABLE) { COOLANT_FLOOD_PORT |= (1 << COOLANT_FLOOD_BIT); diff --git a/gcode.c b/gcode.c index 8557956..cdef3ad 100644 --- a/gcode.c +++ b/gcode.c @@ -62,7 +62,7 @@ void gc_sync_position() { uint8_t i; for (i=0; i. -OBJECTS = main.o simulator.o runtime.o ../protocol.o ../planner.o ../settings.o ../print.o ../nuts_bolts.o eeprom.o serial.o avr/pgmspace.o avr/interrupt.o util/delay.o util/floatunsisf.o ../stepper.o ../gcode.o ../spindle_control.o ../motion_control.o ../limits.o ../report.o ../coolant_control.o +PLATFORM = LINUX + +OBJECTS = main.o simulator.o serial.o ../main.o ../protocol.o ../planner.o ../settings.o ../print.o ../nuts_bolts.o eeprom.o ../serial.o avr/pgmspace.o avr/interrupt.o avr/io.o util/delay.o util/floatunsisf.o ../stepper.o ../gcode.o ../spindle_control.o ../motion_control.o ../limits.o ../report.o ../coolant_control.o ../probe.o ../system.o kbhit.o platform_$(PLATFORM).o CLOCK = 16000000 EXE_NAME = grbl_sim.exe -COMPILE = $(CC) -Wall -Os -DF_CPU=$(CLOCK) -include config.h -I. - +COMPILE = $(CC) -Wall -g -DF_CPU=$(CLOCK) -include config.h -I. -DPLAT_$(PLATFORM) +LINUX_LIBRARIES = -lrt -pthread # symbolic targets: all: main @@ -30,15 +32,18 @@ clean: # file targets: main: $(OBJECTS) - $(COMPILE) -o $(EXE_NAME) $(OBJECTS) -lm + $(COMPILE) -o $(EXE_NAME) $(OBJECTS) -lm -lrt $($(PLATFORM)_LIBRARIES) %.o: %.c - $(COMPILE) -c $< -o $@ - -../protocol.o: ../protocol.c - $(COMPILE) -include rename_execute_runtime.h -c $< -o $@ + $(COMPILE) -c $< -o $@ ../planner.o: ../planner.c $(COMPILE) -include planner_inject_accessors.c -c $< -o $@ +../serial.o: ../serial.c + $(COMPILE) -include serial_hooks.h -c $< -o $@ + +../main.o: ../main.c + $(COMPILE) -include rename_main.h -c $< -o $@ + diff --git a/sim/avr/interrupt.c b/sim/avr/interrupt.c index 10b5225..2bcbe0d 100644 --- a/sim/avr/interrupt.c +++ b/sim/avr/interrupt.c @@ -21,23 +21,115 @@ */ #include "interrupt.h" +#include "io.h" + +//pseudo-Interrupt vector table +isr_fp compa_vect[6]={0}; +isr_fp compb_vect[6]={0}; +isr_fp ovf_vect[6]={0}; + + +void sei() {io.sreg|=SEI;} +void cli() {io.sreg&=~SEI;} + + + +int16_t sim_scaling[8]={0,1,8,64,256,1024,1,1}; //clock scalars +//Timer/Counter modes: these are incomplete, but enough for this application +enum sim_wgm_mode { + wgm_NORMAL, + wgm_CTC, + wgm_FAST_PWM, + wgm_PHASE_PWM, + wgm_PH_F_PWM, + wgm_RESERVED +}; + +enum sim_wgm_mode sim_wgm0[4] = {wgm_NORMAL,wgm_PHASE_PWM,wgm_CTC,wgm_FAST_PWM}; +enum sim_wgm_mode sim_wgmN[8] = {wgm_NORMAL,wgm_PHASE_PWM,wgm_PHASE_PWM,wgm_PH_F_PWM, + wgm_CTC, wgm_FAST_PWM, wgm_FAST_PWM, wgm_FAST_PWM}; + +void timer_interrupts() { + int i; + uint8_t ien = io.sreg&SEI; //interrupts enabled? + io.prescaler++; + + //all clocks + for (i=0;i<2;i++){ + + uint8_t cs = io.tccrb[i]&7; //clock select bits + int16_t increment = sim_scaling[cs]; + //check scaling to see if timer fires + if (increment && (io.prescaler&(increment-1))==0) { + + //select waveform generation mode + enum sim_wgm_mode mode; + if (i==0 || i==2) { //(T0 and T2 are different from rest) + uint8_t wgm = io.tccra[i]&3; //look at low 2 bits + mode = sim_wgm0[wgm]; + } + else { + uint8_t wgm = ((io.tccrb[i]&8)>>1) | (io.tccra[i]&3); //only using 3 bits for now + mode = sim_wgmN[wgm]; + } + + //tick + io.tcnt[i]++; + //comparators + if ((io.timsk[i]&(1< - -// dummy register variables -extern uint16_t timsk0; -extern uint16_t timsk1; -extern uint16_t timsk2; -extern uint16_t tcnt0; -extern uint16_t tcnt2; -extern uint16_t tccr0b; -extern uint16_t tccr0a; -extern uint16_t tccr2a; -extern uint16_t tccr2b; -extern uint16_t tccr1b; -extern uint16_t tccr1a; -extern uint16_t ocr1a; -extern uint16_t ocr2a; -extern uint16_t pcmsk0; -extern uint16_t pcicr; - // macros to turn avr interrupts into regular functions -#define TIMER1_COMPA_vect +//#define TIMER1_COMPA_vect #define ISR(a) void interrupt_ ## a () -// enable interrupts does nothing in the simulation environment +// Stub of the timer interrupt functions we need +void interrupt_TIMER0_COMPA_vect(); +void interrupt_TIMER1_COMPA_vect(); +void interrupt_TIMER0_OVF_vect(); +void interrupt_SERIAL_UDRE(); +void interrupt_SERIAL_RX(); + + +//pseudo-Interrupt vector table +typedef void(*isr_fp)(void); +extern isr_fp compa_vect[6]; +extern isr_fp compb_vect[6]; +extern isr_fp ovf_vect[6]; + + +// enable interrupts now does something in the simulation environment +#define SEI 0x80 void sei(); void cli(); -// dummy macros for interrupt related registers -#define TIMSK0 timsk0 -#define TIMSK1 timsk1 -#define TIMSK2 timsk2 -#define OCR1A ocr1a -#define OCR2A ocr2a -#define OCIE1A 0 -#define OCIE2A 0 -#define TCNT0 tcnt0 -#define TCNT2 tcnt2 -#define TCCR0B tccr0b -#define TCCR0A tccr0a -#define TCCR1A tccr1a -#define TCCR1B tccr1b -#define TCCR2A tccr2a -#define TCCR2B tccr2b -#define CS21 0 -#define CS10 0 -#define WGM13 0 -#define WGM12 0 -#define WGM11 0 -#define WGM10 0 -#define WGM21 0 -#define COM1A0 0 -#define COM1B0 0 -#define TOIE0 0 -#define TOIE2 0 -#define PCICR pcicr +//simulate timer operation +void timer_interrupts(); + + #endif diff --git a/sim/avr/io.c b/sim/avr/io.c new file mode 100644 index 0000000..8290fec --- /dev/null +++ b/sim/avr/io.c @@ -0,0 +1,4 @@ +#include "io.h" + +// dummy register variables +volatile io_sim_t io={{0}}; diff --git a/sim/avr/io.h b/sim/avr/io.h index 885021a..df8a515 100644 --- a/sim/avr/io.h +++ b/sim/avr/io.h @@ -1,5 +1,6 @@ /* - io.h - dummy replacement for the avr include of the same name + interrupt.h - replacement for the avr include of the same name to provide + dummy register variables and macros Part of Grbl Simulator @@ -19,3 +20,182 @@ along with Grbl. If not, see . */ + +#ifndef io_h +#define io_h + +#include + +union hilo16 { + uint16_t w; + struct { + uint8_t l; //TODO: check that these are right order on x86. Doesn't matter for current usage, but might someday + uint8_t h; + }; +}; + +enum { + SIM_A, SIM_B, SIM_C, SIM_D, SIM_E, + SIM_F, SIM_G, SIM_H, SIM_J, SIM_K, SIM_L, + SIM_PORT_COUNT +}; + +#define SIM_N_TIMERS 6 + + +// dummy register variables +typedef struct io_sim { + uint8_t ddr[SIM_PORT_COUNT]; + uint8_t port[SIM_PORT_COUNT]; + uint8_t pin[SIM_PORT_COUNT]; + uint8_t timsk[SIM_N_TIMERS]; + uint16_t ocra[SIM_N_TIMERS]; + uint16_t ocrb[SIM_N_TIMERS]; + uint16_t ocrc[SIM_N_TIMERS]; + uint16_t tcnt[SIM_N_TIMERS]; //tcint0 is really only 8bit + uint8_t tccra[SIM_N_TIMERS]; + uint8_t tccrb[SIM_N_TIMERS]; + uint8_t tifr[SIM_N_TIMERS]; + uint8_t pcicr; + uint8_t pcmsk[3]; + uint8_t ucsr0[3]; + uint8_t udr[3]; + union hilo16 ubrr0; + + uint16_t prescaler; //continuously running + uint8_t sreg; + + +} io_sim_t; +volatile extern io_sim_t io; + + + + +// dummy macros for interrupt related registers +#define PORTA io.port[SIM_A] +#define PORTB io.port[SIM_B] +#define PORTC io.port[SIM_C] +#define PORTD io.port[SIM_D] +#define PORTE io.port[SIM_E] +#define PORTF io.port[SIM_F] +#define PORTG io.port[SIM_G] +#define PORTH io.port[SIM_H] +#define PORTJ io.port[SIM_J] +#define PORTK io.port[SIM_K] +#define PORTL io.port[SIM_L] + +#define DDRA io.ddr[SIM_A] +#define DDRB io.ddr[SIM_B] +#define DDRC io.ddr[SIM_C] +#define DDRD io.ddr[SIM_D] +#define DDRE io.ddr[SIM_E] +#define DDRF io.ddr[SIM_F] +#define DDRG io.ddr[SIM_G] +#define DDRH io.ddr[SIM_H] +#define DDRJ io.ddr[SIM_J] + +#define PINA io.pin[SIM_A] +#define PINB io.pin[SIM_B] +#define PINC io.pin[SIM_C] +#define PIND io.pin[SIM_D] +#define PINE io.pin[SIM_E] +#define PINF io.pin[SIM_F] +#define PING io.pin[SIM_G] +#define PINH io.pin[SIM_H] +#define PINJ io.pin[SIM_J] +#define PINK io.pin[SIM_K] +#define PINL io.pin[SIM_L] + + +#define TIMSK0 io.timsk[0] +#define TIMSK1 io.timsk[1] +#define TIMSK2 io.timsk[2] +#define TIMSK3 io.timsk[3] +#define TIMSK4 io.timsk[4] +#define TIMSK5 io.timsk[5] + + +#define SIM_TOV 0 +#define SIM_OCA 1 +#define SIM_OCB 2 +#define SIM_OCC 3 +#define SIM_ICI 5 + +#define OCIE0A SIM_OCA +#define OCIE0B SIM_OCB +#define TOIE0 SIM_TOV + +#define ICIE1 SIM_ICI +#define OCIE1C SIM_OCC +#define OCIE1B SIM_OCB +#define OCIE1A SIM_OCA +#define TOIE1 SIM_ICI + +#define ICIE2 SIM_ICI +#define OCIE2C SIM_OCC +#define OCIE2B SIM_OCB +#define OCIE2A SIM_OCA +#define TOIE2 SIM_TOV + +#define OCR0A io.ocra[0] +#define OCR1A io.ocra[1] +#define OCR2A io.ocra[2] + //There are more.. + + +#define TCNT0 io.tcnt[0] +#define TCNT1 io.tcnt[1] +#define TCNT2 io.tcnt[2] + +#define TCCR0B io.tccra[0] +#define TCCR0A io.tccrb[0] +#define TCCR1A io.tccra[1] +#define TCCR1B io.tccrb[1] +#define TCCR2A io.tccra[2] +#define TCCR2B io.tccrb[2] + +#define CS00 0 +#define CS01 1 +#define CS12 2 +#define CS11 1 +#define CS10 0 +#define CS21 1 + +#define WGM13 4 +#define WGM12 3 +#define WGM11 1 +#define WGM10 0 +#define WGM21 1 + +#define COM1A1 7 +#define COM1A0 6 +#define COM1B1 5 +#define COM1B0 4 +#define COM1C1 3 +#define COM1C0 2 + + +#define PCICR io.pcicr +#define PCIE0 0 +#define PCIE1 1 + +//serial channel +#define UCSR0A io.ucsr0[SIM_A] +#define UCSR0B io.ucsr0[SIM_B] +#define UDR0 io.udr[0] +#define UDRIE0 0 +#define RXCIE0 1 +#define RXEN0 2 +#define TXEN0 3 +#define U2X0 4 +#define UBRR0H io.ubrr0.h +#define UBRR0L io.ubrr0.l + +#define PCMSK0 io.pcmsk[0] +#define PCMSK1 io.pcmsk[1] +#define PCMSK2 io.pcmsk[2] + + + +#endif diff --git a/sim/avr/wdt.h b/sim/avr/wdt.h new file mode 100644 index 0000000..482f4fd --- /dev/null +++ b/sim/avr/wdt.h @@ -0,0 +1 @@ +uint16_t wdt; diff --git a/sim/config.h b/sim/config.h index 404accd..5baa20c 100644 --- a/sim/config.h +++ b/sim/config.h @@ -24,94 +24,5 @@ #include "../config.h" #include - -// dummy register variables implemented in simulator.c -extern uint8_t stepping_ddr; -extern uint8_t stepping_port; -extern uint8_t spindle_ddr; -extern uint8_t spindle_port; -extern uint8_t limit_ddr; -extern uint8_t limit_port; -extern uint8_t limit_int_reg; -extern uint8_t pinout_ddr; -extern uint8_t pinout_port; -extern uint8_t pinout_int_reg; -extern uint8_t coolant_flood_ddr; -extern uint8_t coolant_flood_port; - -// ReDefine pin-assignments -#undef STEPPING_DDR -#define STEPPING_DDR stepping_ddr -#undef STEPPING_PORT -#define STEPPING_PORT stepping_port -#undef X_STEP_BIT -#define X_STEP_BIT 2 // Uno Digital Pin 2 -#undef Y_STEP_BIT -#define Y_STEP_BIT 3 // Uno Digital Pin 3 -#undef Z_STEP_BIT -#define Z_STEP_BIT 4 // Uno Digital Pin 4 -#undef X_DIRECTION_BIT -#define X_DIRECTION_BIT 5 // Uno Digital Pin 5 -#undef Y_DIRECTION_BIT -#define Y_DIRECTION_BIT 6 // Uno Digital Pin 6 -#undef Z_DIRECTION_BIT -#define Z_DIRECTION_BIT 7 // Uno Digital Pin 7 - -#undef STEPPERS_DISABLE_DDR -#define STEPPERS_DISABLE_DDR stepping_ddr -#undef STEPPERS_DISABLE_PORT -#define STEPPERS_DISABLE_PORT stepping_port -#undef STEPPERS_DISABLE_BIT -#define STEPPERS_DISABLE_BIT 0 // Uno Digital Pin 8 - -#undef LIMIT_DDR -#define LIMIT_DDR limit_ddr -#undef LIMIT_PORT -#define LIMIT_PORT limit_port -#undef LIMIT_PIN -#define LIMIT_PIN limit_port -#undef X_LIMIT_BIT -#define X_LIMIT_BIT 1 // Uno Digital Pin 9 -#undef Y_LIMIT_BIT -#define Y_LIMIT_BIT 2 // Uno Digital Pin 10 -#undef Z_LIMIT_BIT -#define Z_LIMIT_BIT 3 // Uno Digital Pin 11 -#undef LIMIT_INT -#define LIMIT_INT 0 -#undef LIMIT_PCMSK -#define LIMIT_PCMSK limit_int_reg - -#undef SPINDLE_ENABLE_DDR -#define SPINDLE_ENABLE_DDR spindle_ddr -#undef SPINDLE_ENABLE_PORT -#define SPINDLE_ENABLE_PORT spindle_port -#undef SPINDLE_ENABLE_BIT -#define SPINDLE_ENABLE_BIT 4 // Uno Digital Pin 12 - -#undef SPINDLE_DIRECTION_DDR -#define SPINDLE_DIRECTION_DDR spindle_ddr -#undef SPINDLE_DIRECTION_PORT -#define SPINDLE_DIRECTION_PORT spindle_port -#undef SPINDLE_DIRECTION_BIT -#define SPINDLE_DIRECTION_BIT 5 // Uno Digital Pin 13 - -#undef PINOUT_DDR -#define PINOUT_DDR pinout_ddr -#undef PINOUT_PORT -#define PINOUT_PORT pinout_port -#undef PINOUT_PIN -#define PINOUT_PIN pinout_port -#undef PINOUT_PCMSK -#define PINOUT_PCMSK pinout_int_reg -#undef PINOUT_INT -#define PINOUT_INT 0 - -#undef COOLANT_FLOOD_DDR -#define COOLANT_FLOOD_DDR coolant_flood_ddr -#undef COOLANT_FLOOD_PORT -#define COOLANT_FLOOD_PORT coolant_flood_port -#undef COOLANT_FLOOD_BIT -#define COOLANT_FLOOD_BIT 0 - - +#define AUTO_REPORT_MOVE_DONE #endif diff --git a/sim/eeprom.c b/sim/eeprom.c index 5c3c3f3..0291e85 100644 --- a/sim/eeprom.c +++ b/sim/eeprom.c @@ -21,18 +21,75 @@ */ // These are never called in the simulator +#include +#define MAX_EEPROM_SIZE 4096 //4KB EEPROM in Mega + + +FILE* eeprom_create_empty_file(){ + FILE* fp = fopen("EEPROM.DAT","w+b"); + int i; + if (fp){ + for(i=0;i 0; size--) { + checksum = (checksum << 1) || (checksum >> 7); + checksum += *source; + eeprom_put_char(destination++, *(source++)); + } + eeprom_put_char(destination, checksum); } int memcpy_from_eeprom_with_checksum(char *destination, unsigned int source, unsigned int size) { - return 0; + unsigned char data, checksum = 0; + for(; size > 0; size--) { + data = eeprom_get_char(source++); + checksum = (checksum << 1) || (checksum >> 7); + checksum += data; + *(destination++) = data; + } + return(checksum == eeprom_get_char(source)); } // end of file diff --git a/sim/eeprom.h b/sim/eeprom.h new file mode 100644 index 0000000..6b9a562 --- /dev/null +++ b/sim/eeprom.h @@ -0,0 +1,3 @@ + +#include "../eeprom.h" +void eeprom_close(); diff --git a/sim/kbhit.c b/sim/kbhit.c new file mode 100644 index 0000000..7fc6e9e --- /dev/null +++ b/sim/kbhit.c @@ -0,0 +1,6 @@ +#include +#include + +#include "kbhit.h" + + diff --git a/sim/runtime.c b/sim/kbhit.h similarity index 50% rename from sim/runtime.c rename to sim/kbhit.h index 1b38d6a..7ce1a91 100644 --- a/sim/runtime.c +++ b/sim/kbhit.h @@ -1,38 +1,39 @@ -/* - runtime.c - replacement for the modul of the same name in grbl - Run time commands are not processed in the simulator. - Instead, the execute_runtime() is used as a hook to handle stepper simulation - and printing of simulation results. - - Part of Grbl Simulator - - Copyright (c) 2012 Jens Geisler - - 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 - (at your option) any later version. - - Grbl is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with Grbl. If not, see . -*/ - -#include "simulator.h" -#include - -void orig_protocol_execute_runtime(void); - -// replacement for original execute_runtime as a hook to print blocks as they are generated -// and to control simulation of buffered blocks -void protocol_execute_runtime(void) { - orig_protocol_execute_runtime(); - //printf("printBlock():\n"); - printBlock(); - //printf("handle_buffer():\n"); - handle_buffer(); -} +/* + kbhit.h - keyboard hit detection - used in serial port replacement for grbl sim + + linux kbhit taken from http://cboard.cprogramming.com/c-programming/63166-kbhit-linux.html. + By 'thanatos':http://cboard.cprogramming.com/member.php?u=380 + + Part of Grbl Simulator + + Copyright (c) 2014 Adam Shelly + + 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 + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +//if linux +#include +#include +#include + +int kbhit(void); +void enable_kbhit(int); + + +//else +//#include +//#define enable_kbhit(e) +//#define kbhit _kbhit +//endif + diff --git a/sim/main.c b/sim/main.c index d157ac3..e2a4e2e 100644 --- a/sim/main.c +++ b/sim/main.c @@ -34,18 +34,106 @@ #include "simulator.h" +arg_vars_t args; +const char* progname; + +int usage(const char* badarg){ + if (badarg){ + printf("Unrecognized option %s\n",badarg); + } + printf("Usage: \n" + "%s [options] [time_step] [block_file]\n" + " Options:\n" + " -r : minimum time step for printing stepper values. Default=0=no print.\n" + " -t