From 903b462b2e37b18a8f7884924109fecd66c69e27 Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Sun, 29 Dec 2013 20:34:51 -0700 Subject: [PATCH] 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