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

Grbl v1.0e huge beta release. Overrides and new reporting.

Browse Source 
- Feature: Realtime feed, rapid, and spindle speed overrides. These
alter the running machine state within tens of milliseconds!
    - Feed override: 100%, +/-10%, +/-1% commands with values 1-200% of
programmed feed
    - Rapid override: 100%, 50%, 25% rapid rate commands
    - Spindle speed override: 100%, +/-10%, +/-1% commands with values
50-200% of programmed speed
    - Override values have configurable limits and increments in
config.h.
- Feature: Realtime toggle overrides for spindle stop, flood coolant,
and optionally mist coolant
    - Spindle stop: Enables and disables spindle during a feed hold.
Automatically restores last spindles state.
    - Flood and mist coolant: Immediately toggles coolant state until
next toggle or g-code coolant command.
- Feature: Jogging mode! Incremental and absolute modes supported.
    - Grbl accepts jogging-specific commands like $J=X100F50. An axis
word and feed rate are required. G20/21 and G90/G91 commands are
accepted.
    - Jog motions can be canceled at any time by a feed hold `!`
command. The buffer is automatically flushed. (No resetting required).
    - Jog motions do not alter the g-code parser state so GUIs don’t
have to track what they changed and correct it.
- Feature: Laser mode setting. Allows Grbl to execute continuous
motions with spindle speed and state changes.
- Feature: Significantly improved status reports. Overhauled to cram in
more meaningful data and still make it smaller on average.
    - All available data is now sent by default, but does not appear if
it doesn’t change or is not active.
    - Machine position(MPos) or work position(WPos) is reported but not
both at the same time. Instead, the work coordinate offsets (WCO)are
sent intermittently whenever it changes or refreshes after 10-30 status
reports. Position vectors are easily computed by WPos  = MPos - WCO.
    - All data has changed in some way. Details of changes are in the
markdown documents and wiki.
- Feature: 16 new realtime commands to control overrides. All in
extended-ASCII character space.
    - While they are not easily typeable and requires a GUI, they can’t
be accidentally triggered by some latent character in the g-code
program and have tons of room for expansion.
- Feature: New substates for HOLD and SAFETY DOOR. A `:x` is appended
to the state, where `x` is an integer and indicates a substate.
    - For example, each integer of a door state describes in what phase
the machine is in during parking. Substates are detailed in the
documentation.
- Feature: With the alarm codes, homing and probe alarms have been
expanded with more codes to provide more exact feedback on what caused
the alarm.
- Feature: New hard limit check upon power-up or reset. If detected, a
feedback message to check the limit switches sent immediately after the
welcome message.
    - May be disabled in config.h.

- OEM feature: Enable/disable `$RST=` individual commands based on
desired behavior in config.h.
- OEM feature: Configurable EEPROM wipe to prevent certain data from
being deleted during firmware upgrade to a new settings version or
`RST=*` command.
- OEM feature: Enable/disable the `$I=` build info write string with
external EEPROM write example sketch.
    - This prevents a user from altering the build info string in
EEPROM. This requires the vendor to write the string to EEPROM via
external means. An Arduino example sketch is provided to accomplish
this. This would be useful for contain product data that is
retrievable.

- Tweak: All feedback has been drastically trimmed to free up flash
space for the v1.0 release.
    - The `$` help message is just one string, listing available
commands.
    - The `$$` settings printout no longer includes descriptions. Only
the setting values. (Sorry it’s this or remove overrides!)
    - Grbl `error:` and `ALARM:` responses now only contain codes. No
descriptions. All codes are explained in documentation.
    - Grbl’s old feedback style may be restored via a config.h, but
keep in mind that it will likely not fit into the Arduino’s flash space.
- Tweak: Grbl now forces a buffer sync or stop motion whenever a g-code
command needs to update and write a value to EEPROM or changes the work
coordinate offset.
    - This addresses two old issues in all prior Grbl versions. First,
an EEPROM write requires interrupts to be disabled, including stepper
and serial comm. Steps can be lost and data can be corrupted. Second,
the work position may not be correlated to the actual machine position,
since machine position is derived from the actual current execution
state, while work position is based on the g-code parser offset state.
They are usually not in sync and the parser state is several motions
behind. This forced sync ensures work and machine positions are always
correct.
    - This behavior can be disabled through a config.h option, but it’s
not recommended to do so.
- Tweak: To make status reports standardized, users can no longer
change what is reported via status report mask, except for only
toggling machine or work positions.
    - All other data fields are included in the report and can only be
disabled through the config.h file. It’s not recommended to alter this,
because GUIs will be expecting this data to be present and may not be
compatible.
- Tweak: Homing cycle and parking motion no longer report a negative
line number in a status report. These will now not report a line number
at all.
- Tweak: New `[Restoring spindle]` message when restoring from a
spindle stop override. Provides feedback what Grbl is doing while the
spindle is powering up and a 4.0 second delay is enforced.
- Tweak: Override values are reset to 100% upon M2/30. This behavior
can be disabled in config.h
- Tweak: The planner buffer size has been reduced from 18 to 16 to free
up RAM for tracking and controlling overrides.
- Tweak: TX buffer size has been increased from 64 to 90 bytes to
improve status reporting and overall performance.
- Tweak: Removed the MOTION CANCEL state. It was redundant and didn’t
affect Grbl’s overall operation by doing so.
- Tweak: Grbl’s serial buffer increased by +1 internally, such that 128
bytes means 128, not 127 due to the ring buffer implementation. Long
overdue.
- Tweak: Altered sys.alarm variable to be set by alarm codes, rather
than bit flags. Simplified how it worked overall.
- Tweak: Planner buffer and serial RX buffer usage has been combined in
the status reports.
- Tweak: Pin state reporting has been refactored to report only the
pins “triggered” and nothing when not “triggered”.
- Tweak: Current machine rate or speed is now included in every report.
- Tweak: The work coordinate offset (WCO) and override states only need
to be refreshed intermittently or reported when they change. The
refresh rates may be altered for each in the config.h file with
different idle and busy rates to lessen Grbl’s load during a job.
- Tweak: For temporary compatibility to existing GUIs until they are
updated, an option to revert back to the old style status reports is
available in config.h, but not recommended for long term use.
- Tweak: Removed old limit pin state reporting option from config.h in
lieu of new status report that includes them.
- Tweak: Updated the defaults.h file to include laser mode, altered
status report mask, and fix an issue with a missing invert probe pin
default.

- Refactor: Changed how planner line data is generated and passed to
the planner and onto the step generator. By making it a struct
variable, this saved significant flash space.
- Refactor: Major re-factoring of the planner to incorporate override
values and allow for re-calculations fast enough to immediately take
effect during operation. No small feat.
- Refactor: Re-factored the step segment generator for re-computing new
override states.
- Refactor: Re-factored spindle_control.c to accommodate the spindle
speed overrides and laser mode.
- Refactor: Re-factored parts of the codebase for a new jogging mode.
Still under development though and slated to be part of the official
v1.0 release. Hang tight.
- Refactor: Created functions for computing a unit vector and value
limiting based on axis maximums to free up more flash.
- Refactor: The spindle PWM is now set directly inside of the stepper
ISR as it loads new step segments.
- Refactor: Moved machine travel checks out of soft limits function
into its own since jogging uses this too.
- Refactor: Removed coolant_stop() and combined with
coolant_set_state().
- Refactor: The serial RX ISR forks off extended ASCII values to
quickly assess the new override realtime commands.
- Refactor: Altered some names of the step control flags.
- Refactor: Improved efficiency of the serial RX get buffer count
function.
- Refactor: Saved significant flash by removing and combining print
functions. Namely the uint8 base10 and base2 functions.
- Refactor: Moved the probe state check in the main stepper ISR to
improve its efficiency.
- Refactor: Single character printPgmStrings() went converted to direct
serial_write() commands to save significant flash space.

- Documentation: Detailed Markdown documents on error codes, alarm
codes, messages, new real-time commands, new status reports, and how
jogging works. More to come later and will be posted on the Wiki as
well.
- Documentation: CSV files for quick importing of Grbl error and alarm
codes.

- Bug Fix: Applied v0.9 master fixes to CoreXY homing.
- Bug Fix: The print float function would cause Grbl to crash if a
value was 1e6 or greater. Increased the buffer by 3 bytes to help
prevent this in the future.
- Bug Fix: Build info and startup string EEPROM restoring was not
writing the checksum value.
- Bug Fix: Corrected an issue with safety door restoring the proper
spindle and coolant state. It worked before, but breaks with laser mode
that can continually change spindle state per planner block.
- Bug Fix: Move system position and probe position arrays out of the
system_t struct. Ran into some compiling errors that were hard to track
down as to why. Moving them out fixed it.
This commit is contained in:
chamnit
2016-09-21 19:08:24 -06:00
parent 0746a5a1d7
commit 12f48a008a
48 changed files with 3998 additions and 2228 deletions
Download Patch File Download Diff File Expand all files Collapse all files

497
grbl/stepper.c
View File

@ -2,9 +2,9 @@
stepper.c - stepper motor driver: executes motion plans using stepper motors
Part of Grbl
Copyright (c) 2011-2015 Sungeun K. Jeon
Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC
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
@ -23,21 +23,23 @@
// Some useful constants.
#define DT_SEGMENT (1.0/(ACCELERATION_TICKS_PER_SECOND*60.0)) // min/segment
#define REQ_MM_INCREMENT_SCALAR 1.25
#define DT_SEGMENT (1.0/(ACCELERATION_TICKS_PER_SECOND*60.0)) // min/segment
#define REQ_MM_INCREMENT_SCALAR 1.25
#define RAMP_ACCEL 0
#define RAMP_CRUISE 1
#define RAMP_DECEL 2
#define RAMP_DECEL_OVERRIDE 3
#define PREP_FLAG_RECALCULATE bit(0)
#define PREP_FLAG_HOLD_PARTIAL_BLOCK bit(1)
#define PREP_FLAG_PARKING bit(2)
#define PREP_FLAG_DECEL_OVERRIDE bit(3)
// Define Adaptive Multi-Axis Step-Smoothing(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
// 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 cutoff 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
@ -49,22 +51,25 @@
#define AMASS_LEVEL3 (F_CPU/2000) // Over-drives ISR (x8)
// Stores the planner block Bresenham algorithm execution data for the segments in the segment
// 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. Also, AMASS alters this
// data for its own use.
typedef struct {
// data for its own use.
typedef struct {
uint8_t direction_bits;
#ifdef VARIABLE_SPINDLE
uint8_t spindle_pwm;
#endif
uint32_t steps[N_AXIS];
uint32_t step_event_count;
} st_block_t;
static st_block_t st_block_buffer[SEGMENT_BUFFER_SIZE-1];
// Primary stepper segment ring buffer. Contains small, short line segments for the stepper
// 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
// 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
@ -82,12 +87,12 @@ 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_y,
counter_z;
#ifdef STEP_PULSE_DELAY
uint8_t step_bits; // Stores out_bits output to complete the step pulse delay
#endif
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
@ -96,7 +101,7 @@ typedef struct {
uint32_t steps[N_AXIS];
#endif
uint16_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
@ -108,24 +113,24 @@ static volatile uint8_t segment_buffer_tail;
static uint8_t segment_buffer_head;
static uint8_t segment_next_head;
// Step and direction port invert masks.
// Step and direction port invert masks.
static uint8_t step_port_invert_mask;
static uint8_t dir_port_invert_mask;
// Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though.
static volatile uint8_t busy;
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_block; // Pointer to the planner block being prepped
static st_block_t *st_prep_block; // Pointer to the stepper block data 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 recalculate_flag;
float dt_remainder;
float steps_remaining;
float step_per_mm;
@ -150,7 +155,7 @@ typedef struct {
static st_prep_t prep;
/* BLOCK VELOCITY PROFILE DEFINITION
/* BLOCK VELOCITY PROFILE DEFINITION
__________________________
/| |\ _________________ ^
/ | | \ /| |\ |
@ -161,72 +166,70 @@ static st_prep_t prep;
| BLOCK 1 ^ BLOCK 2 | d
|
time -----> EXAMPLE: Block 2 entry speed is at max junction velocity
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
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
deceleration, acceleration-cruise, acceleration-only, deceleration-only, full-trapezoid, and
triangle(no cruise).
maximum_speed (< nominal_speed) -> +
+--------+ <- maximum_speed (= nominal_speed) /|\
/ \ / | \
maximum_speed (< nominal_speed) -> +
+--------+ <- maximum_speed (= nominal_speed) /|\
/ \ / | \
current_speed -> + \ / | + <- exit_speed
| + <- exit_speed / | |
+-------------+ current_speed -> +----+--+
time --> ^ ^ ^ ^
| | | |
| + <- 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
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
// enabled. Startup init and limits call this function but shouldn't start the cycle.
void st_wake_up()
void st_wake_up()
{
// Enable stepper drivers.
if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { STEPPERS_DISABLE_PORT |= (1<<STEPPERS_DISABLE_BIT); }
else { STEPPERS_DISABLE_PORT &= ~(1<<STEPPERS_DISABLE_BIT); }
// if (sys.state & (STATE_CYCLE | STATE_HOMING)){
// Initialize stepper output bits
st.dir_outbits = dir_port_invert_mask;
st.step_outbits = step_port_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.
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
// Initialize stepper output bits
st.dir_outbits = dir_port_invert_mask;
st.step_outbits = step_port_invert_mask;
// Enable Stepper Driver Interrupt
TIMSK1 |= (1<<OCIE1A);
// }
// 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.
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
TIMSK1 |= (1<<OCIE1A);
}
// Stepper shutdown
void st_go_idle()
void st_go_idle()
{
// Disable Stepper Driver Interrupt. Allow Stepper Port Reset Interrupt to finish, if active.
TIMSK1 &= ~(1<<OCIE1A); // Disable Timer1 interrupt
TCCR1B = (TCCR1B & ~((1<<CS12) | (1<<CS11))) | (1<<CS10); // Reset clock to no prescaling.
busy = false;
// Set stepper driver idle state, disabled or enabled, depending on settings and circumstances.
bool pin_state = false; // Keep enabled.
if (((settings.stepper_idle_lock_time != 0xff) || sys_rt_exec_alarm) && sys.state != STATE_HOMING) {
@ -246,54 +249,54 @@ void st_go_idle()
Unlike the popular DDA algorithm, the Bresenham algorithm is not susceptible to numerical
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, 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, for certain multi-axis motions, the non-dominant axes may suffer from un-smooth step
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
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
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
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
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, the dominant axis steps every four ISR ticks, and quadruple the
stepper ISR frequency. And so on. This, in effect, virtually eliminates multi-axis aliasing
issues with the Bresenham algorithm and does not significantly alter Grbl's performance, but
Level 2, we simply bit-shift again, so the non-dominant Bresenham axes can step within any
of the four ISR ticks, the dominant axis steps every four ISR ticks, and quadruple the
stepper ISR frequency. And so on. This, in effect, virtually eliminates 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.
AMASS retains the Bresenham algorithm exactness by requiring that it always executes a full
Bresenham step, regardless of AMASS Level. Meaning that for an AMASS Level 2, all four
intermediate steps must be completed such that baseline Bresenham (Level 0) count is always
retained. Similarly, AMASS Level 3 means all eight intermediate steps must be executed.
Bresenham step, regardless of AMASS Level. Meaning that for an AMASS Level 2, all four
intermediate steps must be completed such that baseline Bresenham (Level 0) count is always
retained. Similarly, AMASS Level 3 means all eight intermediate steps must be executed.
Although the AMASS Levels are in reality arbitrary, where the baseline Bresenham counts can
be multiplied by any integer value, multiplication by powers of two are simply used to ease
CPU overhead with bitshift integer operations.
be multiplied by any integer value, multiplication by powers of two are simply used to ease
CPU overhead with bitshift integer operations.
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
executes them by pulsing the stepper pins appropriately via the Bresenham algorithm. This
defined as constant velocity over n number of steps, from the step segment buffer and then
executes them by pulsing the stepper pins appropriately via the Bresenham algorithm. This
ISR 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 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 must be less than 33.3usec (@30kHz ISR rate). Oscilloscope measured time in
NOTE: This interrupt must be as efficient as possible and complete before the next ISR tick,
which for Grbl must be less than 33.3usec (@30kHz ISR rate). Oscilloscope measured time in
ISR is 5usec typical and 25usec maximum, well below requirement.
NOTE: This ISR expects at least one step to be executed per segment.
*/
// 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
// 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(TIMER1_COMPA_vect)
{
{
// SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT; // Debug: Used to time ISR
if (busy) { return; } // The busy-flag is used to avoid reentering this interrupt
// Set the direction pins a couple of nanoseconds before we step the steppers
DIRECTION_PORT = (DIRECTION_PORT & ~DIRECTION_MASK) | (st.dir_outbits & DIRECTION_MASK);
@ -302,7 +305,7 @@ ISR(TIMER1_COMPA_vect)
st.step_bits = (STEP_PORT & ~STEP_MASK) | st.step_outbits; // Store out_bits to prevent overwriting.
#else // Normal operation
STEP_PORT = (STEP_PORT & ~STEP_MASK) | st.step_outbits;
#endif
#endif
// Enable step pulse reset timer so that The Stepper Port Reset Interrupt can reset the signal after
// exactly settings.pulse_microseconds microseconds, independent of the main Timer1 prescaler.
@ -310,9 +313,9 @@ ISR(TIMER1_COMPA_vect)
TCCR0B = (1<<CS01); // Begin Timer0. Full speed, 1/8 prescaler
busy = true;
sei(); // Re-enable interrupts to allow Stepper Port Reset Interrupt to fire on-time.
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.exec_segment == NULL) {
// Anything in the buffer? If so, load and initialize next step segment.
@ -333,11 +336,11 @@ 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];
// Initialize Bresenham line and distance counters
st.counter_x = st.counter_y = st.counter_z = (st.exec_block->step_event_count >> 1);
}
st.dir_outbits = st.exec_block->direction_bits ^ dir_port_invert_mask;
st.dir_outbits = st.exec_block->direction_bits ^ dir_port_invert_mask;
#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
// With AMASS enabled, adjust Bresenham axis increment counters according to AMASS level.
@ -345,68 +348,73 @@ ISR(TIMER1_COMPA_vect)
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
#ifdef VARIABLE_SPINDLE
// Set real-time spindle output as segment is loaded, just prior to the first step.
spindle_set_speed(st.exec_block->spindle_pwm);
#endif
} else {
// Segment buffer empty. Shutdown.
st_go_idle();
system_set_exec_state_flag(EXEC_CYCLE_STOP); // Flag main program for cycle end
return; // Nothing to do but exit.
}
}
}
// Check probing state.
probe_state_monitor();
if (sys_probe_state == PROBE_ACTIVE) { probe_state_monitor(); }
// Reset step out bits.
st.step_outbits = 0;
st.step_outbits = 0;
// Execute step displacement profile by Bresenham line algorithm
#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
st.counter_x += st.steps[X_AXIS];
#else
st.counter_x += st.exec_block->steps[X_AXIS];
#endif
#endif
if (st.counter_x > st.exec_block->step_event_count) {
st.step_outbits |= (1<<X_STEP_BIT);
st.counter_x -= st.exec_block->step_event_count;
if (st.exec_block->direction_bits & (1<<X_DIRECTION_BIT)) { sys.position[X_AXIS]--; }
else { sys.position[X_AXIS]++; }
if (st.exec_block->direction_bits & (1<<X_DIRECTION_BIT)) { sys_position[X_AXIS]--; }
else { sys_position[X_AXIS]++; }
}
#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
st.counter_y += st.steps[Y_AXIS];
#else
st.counter_y += st.exec_block->steps[Y_AXIS];
#endif
#endif
if (st.counter_y > st.exec_block->step_event_count) {
st.step_outbits |= (1<<Y_STEP_BIT);
st.counter_y -= st.exec_block->step_event_count;
if (st.exec_block->direction_bits & (1<<Y_DIRECTION_BIT)) { sys.position[Y_AXIS]--; }
else { sys.position[Y_AXIS]++; }
if (st.exec_block->direction_bits & (1<<Y_DIRECTION_BIT)) { sys_position[Y_AXIS]--; }
else { sys_position[Y_AXIS]++; }
}
#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
st.counter_z += st.steps[Z_AXIS];
#else
st.counter_z += st.exec_block->steps[Z_AXIS];
#endif
#endif
if (st.counter_z > st.exec_block->step_event_count) {
st.step_outbits |= (1<<Z_STEP_BIT);
st.counter_z -= st.exec_block->step_event_count;
if (st.exec_block->direction_bits & (1<<Z_DIRECTION_BIT)) { sys.position[Z_AXIS]--; }
else { sys.position[Z_AXIS]++; }
}
if (st.exec_block->direction_bits & (1<<Z_DIRECTION_BIT)) { sys_position[Z_AXIS]--; }
else { sys_position[Z_AXIS]++; }
}
// During a homing cycle, lock out and prevent desired axes from moving.
if (sys.state == STATE_HOMING) { st.step_outbits &= sys.homing_axis_lock; }
if (sys.state == STATE_HOMING) { st.step_outbits &= sys.homing_axis_lock; }
st.step_count--; // Decrement step events count
st.step_count--; // Decrement step events count
if (st.step_count == 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.step_outbits ^= step_port_invert_mask; // Apply step port invert mask
st.step_outbits ^= step_port_invert_mask; // Apply step port invert mask
busy = false;
// SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT; // Debug: Used to time ISR
}
@ -417,17 +425,17 @@ ISR(TIMER1_COMPA_vect)
finish, if Timer1 is disabled after completing a move.
NOTE: Interrupt collisions between the serial and stepper interrupts can cause delays by
a few microseconds, if they execute right before one another. Not a big deal, but can
cause issues at high step rates if another high frequency asynchronous interrupt is
cause issues at high step rates if another high frequency asynchronous interrupt is
added to Grbl.
*/
// This interrupt is enabled by ISR_TIMER1_COMPAREA when it sets the motor port bits to execute
// a step. This ISR resets the motor port after a short period (settings.pulse_microseconds)
// a step. This ISR resets the motor port after a short period (settings.pulse_microseconds)
// completing one step cycle.
ISR(TIMER0_OVF_vect)
{
// Reset stepping pins (leave the direction pins)
STEP_PORT = (STEP_PORT & ~STEP_MASK) | (step_port_invert_mask & STEP_MASK);
TCCR0B = 0; // Disable Timer0 to prevent re-entering this interrupt when it's not needed.
STEP_PORT = (STEP_PORT & ~STEP_MASK) | (step_port_invert_mask & STEP_MASK);
TCCR0B = 0; // Disable Timer0 to prevent re-entering this interrupt when it's not needed.
}
#ifdef STEP_PULSE_DELAY
// This interrupt is used only when STEP_PULSE_DELAY is enabled. Here, the step pulse is
@ -435,8 +443,8 @@ ISR(TIMER0_OVF_vect)
// will then trigger after the appropriate settings.pulse_microseconds, as in normal operation.
// The new timing between direction, step pulse, and step complete events are setup in the
// st_wake_up() routine.
ISR(TIMER0_COMPA_vect)
{
ISR(TIMER0_COMPA_vect)
{
STEP_PORT = st.step_bits; // Begin step pulse.
}
#endif
@ -444,7 +452,7 @@ ISR(TIMER0_OVF_vect)
// Generates the step and direction port invert masks used in the Stepper Interrupt Driver.
void st_generate_step_dir_invert_masks()
{
{
uint8_t idx;
step_port_invert_mask = 0;
dir_port_invert_mask = 0;
@ -460,7 +468,7 @@ void st_reset()
{
// Initialize stepper driver idle state.
st_go_idle();
// Initialize stepper algorithm variables.
memset(&prep, 0, sizeof(st_prep_t));
memset(&st, 0, sizeof(stepper_t));
@ -470,9 +478,9 @@ void st_reset()
segment_buffer_head = 0; // empty = tail
segment_next_head = 1;
busy = false;
st_generate_step_dir_invert_masks();
// Initialize step and direction port pins.
STEP_PORT = (STEP_PORT & ~STEP_MASK) | step_port_invert_mask;
DIRECTION_PORT = (DIRECTION_PORT & ~DIRECTION_MASK) | dir_port_invert_mask;
@ -490,11 +498,11 @@ void stepper_init()
// Configure Timer 1: Stepper Driver Interrupt
TCCR1B &= ~(1<<WGM13); // waveform generation = 0100 = CTC
TCCR1B |= (1<<WGM12);
TCCR1A &= ~((1<<WGM11) | (1<<WGM10));
TCCR1A &= ~((1<<WGM11) | (1<<WGM10));
TCCR1A &= ~((1<<COM1A1) | (1<<COM1A0) | (1<<COM1B1) | (1<<COM1B0)); // Disconnect OC1 output
// TCCR1B = (TCCR1B & ~((1<<CS12) | (1<<CS11))) | (1<<CS10); // Set in st_go_idle().
// TIMSK1 &= ~(1<<OCIE1A); // Set in st_go_idle().
// Configure Timer 0: Stepper Port Reset Interrupt
TIMSK0 &= ~((1<<OCIE0B) | (1<<OCIE0A) | (1<<TOIE0)); // Disconnect OC0 outputs and OVF interrupt.
TCCR0A = 0; // Normal operation
@ -504,11 +512,11 @@ void stepper_init()
TIMSK0 |= (1<<OCIE0A); // Enable Timer0 Compare Match A interrupt
#endif
}
// 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.
prep.recalculate_flag |= PREP_FLAG_RECALCULATE;
pl_block->entry_speed_sqr = prep.current_speed*prep.current_speed; // Update entry speed.
@ -545,10 +553,9 @@ static uint8_t st_next_block_index(uint8_t block_index)
// Restores the step segment buffer to the normal run state after a parking motion.
// NOTE: This function does not compile if parking is disabled.
void st_parking_restore_buffer()
{
// Restore step execution data and flags of partially completed block, if necessary.
{
// Restore step execution data and flags of partially completed block, if necessary.
if (prep.recalculate_flag & PREP_FLAG_HOLD_PARTIAL_BLOCK) {
st_prep_block = &st_block_buffer[prep.last_st_block_index];
prep.st_block_index = prep.last_st_block_index;
@ -565,7 +572,7 @@ static uint8_t st_next_block_index(uint8_t block_index)
#endif
/* Prepares step segment buffer. Continuously called from main program.
/* 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
@ -574,7 +581,7 @@ static uint8_t st_next_block_index(uint8_t block_index)
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.
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-50 msec of steps.
NOTE: Computation units are in steps, millimeters, and minutes.
*/
@ -588,42 +595,29 @@ void st_prep_buffer()
// Determine if we need to load a new planner block or if the block needs to be recomputed.
if (pl_block == NULL) {
#ifdef PARKING_ENABLE
// Query planner for a queued block
if (sys.step_control & STEP_CONTROL_EXECUTE_PARK) { pl_block = plan_get_parking_block(); }
else { pl_block = plan_get_current_block(); }
if (pl_block == NULL) { return; } // No planner blocks. Exit.
// Query planner for a queued block
if (sys.step_control & STEP_CONTROL_EXECUTE_SYS_MOTION) { pl_block = plan_get_system_motion_block(); }
else { pl_block = plan_get_current_block(); }
if (pl_block == NULL) { return; } // No planner blocks. Exit.
// Check if we need to only recompute the velocity profile or load a new block.
if (prep.recalculate_flag & PREP_FLAG_RECALCULATE) {
// Check if we need to only recompute the velocity profile or load a new block.
if (prep.recalculate_flag & PREP_FLAG_RECALCULATE) {
#ifdef PARKING_ENABLE
if (prep.recalculate_flag & PREP_FLAG_PARKING) { prep.recalculate_flag &= ~(PREP_FLAG_RECALCULATE); }
else { prep.recalculate_flag = false; }
} else {
#else
// Query planner for a queued block
pl_block = plan_get_current_block();
if (pl_block == NULL) { return; } // No planner blocks. Exit.
// Check if we need to only recompute the velocity profile or load a new block.
if (prep.recalculate_flag & PREP_FLAG_RECALCULATE) {
#else
prep.recalculate_flag = false;
} else {
#endif
#endif
} else {
// Load the Bresenham stepping data for the block.
prep.st_block_index = st_next_block_index(prep.st_block_index);
// 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 when the
// segment buffer finishes the prepped block, but the stepper ISR is still executing it.
// when the segment buffer completes the planner block, it may be discarded when the
// segment buffer finishes the prepped block, but the stepper ISR is still executing it.
st_prep_block = &st_block_buffer[prep.st_block_index];
st_prep_block->direction_bits = pl_block->direction_bits;
uint8_t idx;
@ -631,32 +625,33 @@ void st_prep_buffer()
for (idx=0; idx<N_AXIS; idx++) { st_prep_block->steps[idx] = pl_block->steps[idx]; }
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
// 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.
for (idx=0; idx<N_AXIS; idx++) { st_prep_block->steps[idx] = pl_block->steps[idx] << 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.
prep.steps_remaining = (float)pl_block->step_event_count;
prep.step_per_mm = prep.steps_remaining/pl_block->millimeters;
prep.req_mm_increment = REQ_MM_INCREMENT_SCALAR/prep.step_per_mm;
prep.dt_remainder = 0.0; // Reset for new segment block
if (sys.step_control & STEP_CONTROL_EXECUTE_HOLD) {
if ((sys.step_control & STEP_CONTROL_EXECUTE_HOLD) || (prep.recalculate_flag & PREP_FLAG_DECEL_OVERRIDE)) {
// New block loaded mid-hold. Override planner block entry speed to enforce deceleration.
prep.current_speed = prep.exit_speed;
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);
prep.recalculate_flag &= ~(PREP_FLAG_DECEL_OVERRIDE);
} else {
prep.current_speed = sqrt(pl_block->entry_speed_sqr);
}
}
/* ---------------------------------------------------------------------------------
/* ---------------------------------------------------------------------------------
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
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; // Default velocity profile complete at 0.0mm from end of block.
@ -677,47 +672,77 @@ void st_prep_buffer()
} 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;
prep.accelerate_until = pl_block->millimeters;
#ifdef PARKING_ENABLE
if (sys.step_control & STEP_CONTROL_EXECUTE_PARK) { prep.exit_speed = 0.0; }
else { prep.exit_speed = plan_get_exec_block_exit_speed(); }
#else
prep.exit_speed = plan_get_exec_block_exit_speed();
#endif
float exit_speed_sqr;
float nominal_speed;
if (sys.step_control & STEP_CONTROL_EXECUTE_SYS_MOTION) {
prep.exit_speed = exit_speed_sqr = 0.0; // Enforce stop at end of system motion.
} else {
exit_speed_sqr = plan_get_exec_block_exit_speed_sqr();
prep.exit_speed = sqrt(exit_speed_sqr);
}
float exit_speed_sqr = prep.exit_speed*prep.exit_speed;
nominal_speed = plan_compute_profile_nominal_speed(pl_block);
float nominal_speed_sqr = nominal_speed*nominal_speed;
float intersect_distance =
0.5*(pl_block->millimeters+inv_2_accel*(pl_block->entry_speed_sqr-exit_speed_sqr));
if (intersect_distance > 0.0) {
if (pl_block->entry_speed_sqr > nominal_speed_sqr) { // Only occurs during override reductions.
prep.accelerate_until = pl_block->millimeters - inv_2_accel*(pl_block->entry_speed_sqr-nominal_speed_sqr);
if (prep.accelerate_until <= 0.0) { // Deceleration-only.
prep.ramp_type = RAMP_DECEL;
// prep.decelerate_after = pl_block->millimeters;
// prep.maximum_speed = prep.current_speed;
// Compute override block exit speed since it doesn't match the planner exit speed.
prep.exit_speed = sqrt(pl_block->entry_speed_sqr - 2*pl_block->acceleration*pl_block->millimeters);
prep.recalculate_flag |= PREP_FLAG_DECEL_OVERRIDE; // Flag to load next block as deceleration override.
// TODO: Determine correct handling of parameters in deceleration-only.
// Can be tricky since entry speed will be current speed, as in feed holds.
// Also, look into near-zero speed handling issues with this.
} else {
// Decelerate to cruise or cruise-decelerate types. Guaranteed to intersect updated plan.
prep.decelerate_after = inv_2_accel*(nominal_speed_sqr-exit_speed_sqr);
prep.maximum_speed = nominal_speed;
prep.ramp_type = RAMP_DECEL_OVERRIDE;
}
} else if (intersect_distance > 0.0) {
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);
prep.decelerate_after = inv_2_accel*(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) {
prep.maximum_speed = nominal_speed;
if (pl_block->entry_speed_sqr == 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);
prep.accelerate_until -= inv_2_accel*(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 = pl_block->millimeters;
// prep.maximum_speed = prep.current_speed;
prep.ramp_type = RAMP_DECEL;
// 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.maximum_speed = prep.exit_speed;
}
}
}
#ifdef VARIABLE_SPINDLE
st_prep_block->spindle_pwm = spindle_compute_pwm_value((0.01*sys.spindle_speed_ovr)*pl_block->spindle_speed);
#endif
}
// Initialize new segment
@ -728,30 +753,44 @@ 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 following code first attempts to create
a full segment based on the current ramp conditions. If the segment time is incomplete
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
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,
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; // Maximum segment time
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; // 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 {
switch (prep.ramp_type) {
case RAMP_ACCEL:
case RAMP_DECEL_OVERRIDE:
speed_var = pl_block->acceleration*time_var;
mm_var = time_var*(prep.current_speed - 0.5*speed_var);
mm_remaining -= mm_var;
if ((mm_remaining < prep.accelerate_until) || (mm_var <= 0)) {
// Cruise or cruise-deceleration types only for deceleration override.
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);
prep.ramp_type = RAMP_CRUISE;
prep.current_speed = prep.maximum_speed;
} else { // Mid-deceleration override ramp.
prep.current_speed -= speed_var;
}
break;
case RAMP_ACCEL:
// NOTE: Acceleration ramp only computes during first do-while loop.
speed_var = pl_block->acceleration*time_var;
mm_remaining -= time_var*(prep.current_speed + 0.5*speed_var);
@ -762,23 +801,23 @@ void st_prep_buffer()
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.
} else { // Acceleration only.
prep.current_speed += speed_var;
}
break;
case RAMP_CRUISE:
case RAMP_CRUISE:
// NOTE: mm_var used to retain the last mm_remaining for incomplete segment time_var calculations.
// NOTE: If maximum_speed*time_var value is too low, round-off can cause mm_var to not change. To
// NOTE: If maximum_speed*time_var value is too low, round-off can cause mm_var to not change. To
// prevent this, simply enforce a minimum speed threshold in the planner.
mm_var = mm_remaining - prep.maximum_speed*time_var;
if (mm_var < prep.decelerate_after) { // End of cruise.
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;
}
} else { // Cruising only.
mm_remaining = mm_var;
}
break;
default: // case RAMP_DECEL:
// NOTE: mm_var used as a misc worker variable to prevent errors when near zero speed.
@ -794,7 +833,7 @@ void st_prep_buffer()
}
// Otherwise, at 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;
mm_remaining = prep.mm_complete;
prep.current_speed = prep.exit_speed;
}
dt += time_var; // Add computed ramp time to total segment time.
@ -805,19 +844,19 @@ void st_prep_buffer()
// through distance calculations until minimum_mm or mm_complete.
dt_max += DT_SEGMENT;
time_var = dt_max - dt;
} else {
} else {
break; // **Complete** Exit loop. Segment execution time maxed.
}
}
} while (mm_remaining > prep.mm_complete); // **Complete** Exit loop. Profile complete.
/* -----------------------------------------------------------------------------------
Compute segment step rate, steps to execute, and apply necessary rate corrections.
NOTE: Steps are computed by direct scalar conversion of the millimeter distance
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
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).
@ -826,48 +865,48 @@ void st_prep_buffer()
float n_steps_remaining = ceil(step_dist_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 (prep_segment->n_step == 0) {
if (sys.step_control & STEP_CONTROL_EXECUTE_HOLD) {
// Less than one step to decelerate to zero speed, but already very close. AMASS
// Less than one step to decelerate to zero speed, but already very close. AMASS
// requires full steps to execute. So, just bail.
bit_true(sys.step_control,STEP_CONTROL_END_MOTION);
#ifdef PARKING_ENABLE
if (!(prep.recalculate_flag & PREP_FLAG_PARKING)) { prep.recalculate_flag |= PREP_FLAG_HOLD_PARTIAL_BLOCK; }
if (!(prep.recalculate_flag & PREP_FLAG_PARKING)) { prep.recalculate_flag |= PREP_FLAG_HOLD_PARTIAL_BLOCK; }
#endif
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
// 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
// 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 - step_dist_remaining); // Compute adjusted step rate inverse
// Compute CPU cycles per step for the prepped segment.
uint32_t cycles = ceil( (TICKS_PER_MICROSECOND*1000000*60)*inv_rate ); // (cycles/step)
uint32_t cycles = ceil( (TICKS_PER_MICROSECOND*1000000*60)*inv_rate ); // (cycles/step)
#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
#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.
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;
else { prep_segment->amass_level = 3; }
cycles >>= prep_segment->amass_level;
prep_segment->n_step <<= prep_segment->amass_level;
}
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
#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
@ -875,7 +914,7 @@ void st_prep_buffer()
} else if (cycles < (1UL << 19)) { // < 524288 (32.8ms@16MHz)
prep_segment->prescaler = 2; // prescaler: 8
prep_segment->cycles_per_tick = cycles >> 3;
} else {
} else {
prep_segment->prescaler = 3; // prescaler: 64
if (cycles < (1UL << 22)) { // < 4194304 (262ms@16MHz)
prep_segment->cycles_per_tick = cycles >> 6;
@ -890,16 +929,16 @@ void st_prep_buffer()
if ( ++segment_next_head == SEGMENT_BUFFER_SIZE ) { segment_next_head = 0; }
// Update the appropriate planner and segment data.
pl_block->millimeters = mm_remaining;
pl_block->millimeters = mm_remaining;
prep.steps_remaining = n_steps_remaining;
prep.dt_remainder = (n_steps_remaining - step_dist_remaining)*inv_rate;
// Check for exit conditions and flag to load next planner block.
if (mm_remaining == prep.mm_complete) {
if (mm_remaining == prep.mm_complete) {
// End of planner block or forced-termination. No more distance to be executed.
if (mm_remaining > 0.0) { // At end of forced-termination.
// Reset prep parameters for resuming and then bail. Allow the stepper ISR to complete
// the segment queue, where realtime protocol will set new state upon receiving the
// the segment queue, where realtime protocol will set new state upon receiving the
// cycle stop flag from the ISR. Prep_segment is blocked until then.
bit_true(sys.step_control,STEP_CONTROL_END_MOTION);
#ifdef PARKING_ENABLE
@ -908,31 +947,27 @@ void st_prep_buffer()
return; // Bail!
} else { // End of planner block
// The planner block is complete. All steps are set to be executed in the segment buffer.
#ifdef PARKING_ENABLE
if (sys.step_control & STEP_CONTROL_EXECUTE_PARK) {
bit_true(sys.step_control,STEP_CONTROL_END_MOTION);
return;
}
#endif
if (sys.step_control & STEP_CONTROL_EXECUTE_SYS_MOTION) {
bit_true(sys.step_control,STEP_CONTROL_END_MOTION);
return;
}
pl_block = NULL; // Set pointer to indicate check and load next planner block.
plan_discard_current_block();
}
}
}
}
}
}
// Called by realtime status reporting to fetch the current speed being executed. This value
// however is not exactly the current speed, but the speed computed in the last step segment
// in the segment buffer. It will always be behind by up to the number of segment blocks (-1)
// divided by the ACCELERATION TICKS PER SECOND in seconds.
#ifdef REPORT_REALTIME_RATE
float st_get_realtime_rate()
{
if (sys.state & (STATE_CYCLE | STATE_HOMING | STATE_HOLD | STATE_MOTION_CANCEL | STATE_SAFETY_DOOR)){
return prep.current_speed;
}
return 0.0f;
}
#endif
// divided by the ACCELERATION TICKS PER SECOND in seconds.
float st_get_realtime_rate()
{
if (sys.state & (STATE_CYCLE | STATE_HOMING | STATE_HOLD | STATE_JOG | STATE_SAFETY_DOOR)){
return prep.current_speed;
}
return 0.0f;
}