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

Cleaned up stepper and planner code.

Browse Source 
- Added some compile-time error checking. Will add more in future
pushes to ensure settings are correct and within parameters that won't
break anything.

- Pushed some master branch changes with MEGA pin settings.

- Cleaned up planner code and comments to clarify some of the new
changes. Still much to do here.

- Cleaned up the new stepper code. May need to abstract some of the
segment buffer more to fix the feed holds (and integrate homing into
the main stepper routine). With what's planned, this should make the
stepper algorithm easier to attach other types of processes to it,
where it is now tightly integrated with the planner buffer and nothing
else.
This commit is contained in:
Sonny Jeon 2013-10-24 22:12:13 -06:00
parent 0cb5756b53
commit f7429ec79b
6 changed files with 252 additions and 253 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
README.md
View File

@ -11,7 +11,7 @@ Grbl includes full acceleration management with look ahead. That means the contr
##Changelog for v0.9 from v0.8 ##Changelog for v0.9 from v0.8
- **ALPHA status: Under heavy development.** - **ALPHA status: Under heavy development.**
- New stepper algorithm: Based on the Pramod Ranade inverse time algorithm, but modified to ensure steps are executed exactly. This algorithm performs a constant timer tick and has a hard limit of 30kHz maximum step frequency. It is also highly tuneable and should be very easy to port to other microcontroller architectures. Overall, a much better, smoother stepper algorithm with the capability of very high speeds. - New stepper algorithm: Based on an inverse time algorithm, but modified to ensure steps are executed exactly. This algorithm performs a constant timer tick and has a hard limit of 30kHz maximum step frequency. It is also highly tuneable and should be very easy to port to other microcontroller architectures. Overall, a much better, smoother stepper algorithm with the capability of very high speeds.
- Planner optimizations: Multiple changes to increase planner execution speed and removed redundant variables. - Planner optimizations: Multiple changes to increase planner execution speed and removed redundant variables.
- Acceleration independence: Each axes may be defined with different acceleration parameters and Grbl will automagically calculate the maximum acceleration through a path depending on the direction traveled. This is very useful for machine that have very different axes properties, like the ShapeOko z-axis. - Acceleration independence: Each axes may be defined with different acceleration parameters and Grbl will automagically calculate the maximum acceleration through a path depending on the direction traveled. This is very useful for machine that have very different axes properties, like the ShapeOko z-axis.
- Maximum velocity independence: As with acceleration, the maximum velocity of individual axes may be defined. All seek/rapids motions will move at these maximum rates, but never exceed any one axes. So, when two or more axes move, the limiting axis will move at its maximum rate, while the other axes are scaled down. - Maximum velocity independence: As with acceleration, the maximum velocity of individual axes may be defined. All seek/rapids motions will move at these maximum rates, but never exceed any one axes. So, when two or more axes move, the limiting axis will move at its maximum rate, while the other axes are scaled down.

24
config.h
View File

@ -49,9 +49,9 @@
#define CMD_CYCLE_START '~' #define CMD_CYCLE_START '~'
#define CMD_RESET 0x18 // ctrl-x #define CMD_RESET 0x18 // ctrl-x
// The "Stepper Driver Interrupt" employs the Pramod Ranade inverse time algorithm to manage the // The "Stepper Driver Interrupt" employs an inverse time algorithm to manage the Bresenham line
// Bresenham line stepping algorithm. The value ISR_TICKS_PER_SECOND is the frequency(Hz) at which // stepping algorithm. The value ISR_TICKS_PER_SECOND is the frequency(Hz) at which the inverse time
// the Ranade algorithm ticks at. Recommended step frequencies are limited by the Ranade frequency by // 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 // 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 // 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 // interrupt of the current stepper driver algorithm theoretically up to a frequency of 35-40kHz, but
@ -71,7 +71,7 @@
// NOTE: Make sure this value is less than 256, when adjusting both dependent parameters. // 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) #define ISR_TICKS_PER_ACCELERATION_TICK (ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND)
// The Ranade algorithm can use either floating point or long integers for its counters, but for // The inverse time algorithm can use either floating point or long integers for its counters, but for
// integers the counter values must be scaled since these values can be very small (10^-6). This // integers the counter values must be scaled since these values can be very small (10^-6). This
// multiplier value scales the floating point counter values for use in a long integer. Long integers // multiplier value scales the floating point counter values for use in a long integer. Long integers
// are finite so select the multiplier value high enough to avoid any numerical round-off issues and // are finite so select the multiplier value high enough to avoid any numerical round-off issues and
@ -82,10 +82,10 @@
#define INV_TIME_MULTIPLIER 10000000.0 #define INV_TIME_MULTIPLIER 10000000.0
// Minimum stepper rate for the "Stepper Driver Interrupt". Sets the absolute minimum stepper rate // Minimum stepper rate for the "Stepper Driver Interrupt". Sets the absolute minimum stepper rate
// in the stepper program and never runs slower than this value. If the RANADE_MULTIPLIER value // in the stepper program and never runs slower than this value. If the INVE_TIME_MULTIPLIER value
// changes, it will affect how this value works. So, if a zero is add/subtracted from the // changes, it will affect how this value works. So, if a zero is add/subtracted from the
// RANADE_MULTIPLIER value, do the same to this value if you want to same response. // INV_TIME_MULTIPLIER value, do the same to this value if you want to same response.
// NOTE: Compute by (desired_step_rate/60) * RANADE_MULTIPLIER/ISR_TICKS_PER_SECOND. (mm/min) // NOTE: Compute by (desired_step_rate/60) * INV_TIME_MULTIPLIER/ISR_TICKS_PER_SECOND. (mm/min)
#define MINIMUM_STEP_RATE 1000L // Integer (mult*mm/isr_tic) #define MINIMUM_STEP_RATE 1000L // Integer (mult*mm/isr_tic)
// Minimum stepper rate. Only used by homing at this point. May be removed in later releases. // Minimum stepper rate. Only used by homing at this point. May be removed in later releases.
@ -158,7 +158,7 @@
// available RAM, like when re-compiling for a Mega or Sanguino. Or decrease if the Arduino // available RAM, like when re-compiling for a Mega or Sanguino. Or decrease if the Arduino
// begins to crash due to the lack of available RAM or if the CPU is having trouble keeping // begins to crash due to the lack of available RAM or if the CPU is having trouble keeping
// up with planning new incoming motions as they are executed. // up with planning new incoming motions as they are executed.
// #define BLOCK_BUFFER_SIZE 17 // Uncomment to override default in planner.h. // #define BLOCK_BUFFER_SIZE 18 // Uncomment to override default in planner.h.
// Line buffer size from the serial input stream to be executed. Also, governs the size of // 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 // each of the startup blocks, as they are each stored as a string of this size. Make sure
@ -193,4 +193,12 @@
// TODO: Install compile-time option to send numeric status codes rather than strings. // TODO: Install compile-time option to send numeric status codes rather than strings.
// ---------------------------------------------------------------------------------------
// COMPILE-TIME ERROR CHECKING OF DEFINE VALUES:
#if (ISR_TICKS_PER_ACCELERATION_TICK > 255)
#error Parameters ACCELERATION_TICKS / ISR_TICKS must be < 256 to prevent integer overflow.
#endif
// ---------------------------------------------------------------------------------------
#endif #endif

65
pin_map.h
View File

@ -97,41 +97,44 @@
#endif #endif
#ifdef PIN_MAP_ARDUINO_MEGA_2560 // Unsupported. Doesn't work. Supplied by @elmom. #ifdef PIN_MAP_ARDUINO_MEGA_2560 // Working @EliteEng
// Serial port pins // Serial port pins
#define SERIAL_RX USART0_RX_vect #define SERIAL_RX USART0_RX_vect
#define SERIAL_UDRE USART0_UDRE_vect #define SERIAL_UDRE USART0_UDRE_vect
// Increase Buffers to make use of extra SRAM
#define RX_BUFFER_SIZE 256
#define TX_BUFFER_SIZE 128
#define BLOCK_BUFFER_SIZE 36
#define LINE_BUFFER_SIZE 100
// NOTE: All step bit and direction pins must be on the same port. // NOTE: All step bit and direction pins must be on the same port.
#define STEPPING_DDR DDRA #define STEPPING_DDR DDRA
#define STEPPING_PORT PORTA #define STEPPING_PORT PORTA
#define STEPPING_PIN PINA #define STEPPING_PIN PINA
#define X_STEP_BIT 0 // MEGA2560 Digital Pin 22 #define X_STEP_BIT 2 // MEGA2560 Digital Pin 24
#define Y_STEP_BIT 1 // MEGA2560 Digital Pin 23 #define Y_STEP_BIT 3 // MEGA2560 Digital Pin 25
#define Z_STEP_BIT 2 // MEGA2560 Digital Pin 24 #define Z_STEP_BIT 4 // MEGA2560 Digital Pin 26
// #define C_STEP_BIT 3 // MEGA2560 Digital Pin 25 #define X_DIRECTION_BIT 5 // MEGA2560 Digital Pin 27
#define X_DIRECTION_BIT 4 // MEGA2560 Digital Pin 26 #define Y_DIRECTION_BIT 6 // MEGA2560 Digital Pin 28
#define Y_DIRECTION_BIT 5 // MEGA2560 Digital Pin 27 #define Z_DIRECTION_BIT 7 // MEGA2560 Digital Pin 29
#define Z_DIRECTION_BIT 6 // MEGA2560 Digital Pin 28
// #define C_DIRECTION_BIT 7 // MEGA2560 Digital Pin 29
#define STEP_MASK ((1<<X_STEP_BIT)|(1<<Y_STEP_BIT)|(1<<Z_STEP_BIT)) // All step bits #define STEP_MASK ((1<<X_STEP_BIT)|(1<<Y_STEP_BIT)|(1<<Z_STEP_BIT)) // All step bits
#define DIRECTION_MASK ((1<<X_DIRECTION_BIT)|(1<<Y_DIRECTION_BIT)|(1<<Z_DIRECTION_BIT)) // All direction bits #define DIRECTION_MASK ((1<<X_DIRECTION_BIT)|(1<<Y_DIRECTION_BIT)|(1<<Z_DIRECTION_BIT)) // All direction bits
#define STEPPING_MASK (STEP_MASK | DIRECTION_MASK) // All stepping-related bits (step/direction) #define STEPPING_MASK (STEP_MASK | DIRECTION_MASK) // All stepping-related bits (step/direction)
#define STEPPERS_DISABLE_DDR DDRC #define STEPPERS_DISABLE_DDR DDRB
#define STEPPERS_DISABLE_PORT PORTC #define STEPPERS_DISABLE_PORT PORTB
#define STEPPERS_DISABLE_BIT 7 // MEGA2560 Digital Pin 30 #define STEPPERS_DISABLE_BIT 7 // MEGA2560 Digital Pin 13
#define STEPPERS_DISABLE_MASK (1<<STEPPERS_DISABLE_BIT) #define STEPPERS_DISABLE_MASK (1<<STEPPERS_DISABLE_BIT)
// NOTE: All limit bit pins must be on the same port // NOTE: All limit bit pins must be on the same port
#define LIMIT_DDR DDRC #define LIMIT_DDR DDRB
#define LIMIT_PORT PORTC #define LIMIT_PORT PORTB
#define LIMIT_PIN PINC #define LIMIT_PIN PINB
#define X_LIMIT_BIT 6 // MEGA2560 Digital Pin 31 #define X_LIMIT_BIT 4 // MEGA2560 Digital Pin 10
#define Y_LIMIT_BIT 5 // MEGA2560 Digital Pin 32 #define Y_LIMIT_BIT 5 // MEGA2560 Digital Pin 11
#define Z_LIMIT_BIT 4 // MEGA2560 Digital Pin 33 #define Z_LIMIT_BIT 6 // MEGA2560 Digital Pin 12
// #define C_LIMIT_BIT 3 // MEGA2560 Digital Pin 34
#define LIMIT_INT PCIE0 // Pin change interrupt enable pin #define LIMIT_INT PCIE0 // Pin change interrupt enable pin
#define LIMIT_INT_vect PCINT0_vect #define LIMIT_INT_vect PCINT0_vect
#define LIMIT_PCMSK PCMSK0 // Pin change interrupt register #define LIMIT_PCMSK PCMSK0 // Pin change interrupt register
@ -153,19 +156,19 @@
#ifdef ENABLE_M7 #ifdef ENABLE_M7
#define COOLANT_MIST_DDR DDRC #define COOLANT_MIST_DDR DDRC
#define COOLANT_MIST_PORT PORTC #define COOLANT_MIST_PORT PORTC
#define COOLANT_MIST_BIT 0 // MEGA2560 Digital Pin 37 #define COOLANT_MIST_BIT 3 // MEGA2560 Digital Pin 34
#endif #endif
// NOTE: All pinouts pins must be on the same port // NOTE: All pinouts pins must be on the same port
#define PINOUT_DDR DDRC #define PINOUT_DDR DDRK
#define PINOUT_PIN PINC #define PINOUT_PIN PINK
#define PINOUT_PORT PORTC #define PINOUT_PORT PORTK
#define PIN_RESET 0 // Uno Analog Pin 0 #define PIN_RESET 0 // MEGA2560 Analog Pin 8
#define PIN_FEED_HOLD 1 // Uno Analog Pin 1 #define PIN_FEED_HOLD 1 // MEGA2560 Analog Pin 9
#define PIN_CYCLE_START 2 // Uno Analog Pin 2 #define PIN_CYCLE_START 2 // MEGA2560 Analog Pin 10
#define PINOUT_INT PCIE1 // Pin change interrupt enable pin #define PINOUT_INT PCIE2 // Pin change interrupt enable pin
#define PINOUT_INT_vect PCINT1_vect #define PINOUT_INT_vect PCINT2_vect
#define PINOUT_PCMSK PCMSK1 // Pin change interrupt register #define PINOUT_PCMSK PCMSK2 // Pin change interrupt register
#define PINOUT_MASK ((1<<PIN_RESET)|(1<<PIN_FEED_HOLD)|(1<<PIN_CYCLE_START)) #define PINOUT_MASK ((1<<PIN_RESET)|(1<<PIN_FEED_HOLD)|(1<<PIN_CYCLE_START))
#endif #endif

300
planner.c
View File

@ -51,9 +51,9 @@ typedef struct {
static planner_t pl; static planner_t pl;
// Returns the index of the next block in the ring buffer // Returns the index of the next block in the ring buffer. Also called by stepper segment buffer.
// NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication. // NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication.
static uint8_t next_block_index(uint8_t block_index) uint8_t plan_next_block_index(uint8_t block_index)
{ {
block_index++; block_index++;
if (block_index == BLOCK_BUFFER_SIZE) { block_index = 0; } if (block_index == BLOCK_BUFFER_SIZE) { block_index = 0; }
@ -62,7 +62,7 @@ static uint8_t next_block_index(uint8_t block_index)
// Returns the index of the previous block in the ring buffer // Returns the index of the previous block in the ring buffer
static uint8_t prev_block_index(uint8_t block_index) static uint8_t plan_prev_block_index(uint8_t block_index)
{ {
if (block_index == 0) { block_index = BLOCK_BUFFER_SIZE; } if (block_index == 0) { block_index = BLOCK_BUFFER_SIZE; }
block_index--; block_index--;
@ -70,8 +70,6 @@ static uint8_t prev_block_index(uint8_t block_index)
} }
// Update the entry speed and millimeters remaining to execute for a partially completed block. Called only // Update the entry speed and millimeters remaining to execute for a partially completed block. Called only
// when the planner knows it will be changing the conditions of this block. // when the planner knows it will be changing the conditions of this block.
// TODO: Set up to be called from planner calculations. Need supporting code framework still, i.e. checking // TODO: Set up to be called from planner calculations. Need supporting code framework still, i.e. checking
@ -109,8 +107,6 @@ void plan_update_partial_block(uint8_t block_index, float exit_speed_sqr)
} }
/* PLANNER SPEED DEFINITION /* PLANNER SPEED DEFINITION
+--------+ <- current->nominal_speed +--------+ <- current->nominal_speed
/ \ / \
@ -119,55 +115,41 @@ void plan_update_partial_block(uint8_t block_index, float exit_speed_sqr)
+-------------+ +-------------+
time --> time -->
Recalculates the motion plan according to the following algorithm: Recalculates the motion plan according to the following basic guidelines:
1. Go over every block in reverse order and calculate a junction speed reduction (i.e. block_t.entry_speed) 1. Go over every feasible block sequentially in reverse order and calculate the junction speeds
so that: (i.e. current->entry_speed) such that:
a. The junction speed is equal to or less than the maximum junction speed limit a. No junction speed exceeds the pre-computed maximum junction speed limit or nominal speeds of
b. No speed reduction within one block requires faster deceleration than the acceleration limits. neighboring blocks.
c. The last (or newest appended) block is planned from a complete stop. b. A block entry speed cannot exceed one reverse-computed from its exit speed (next->entry_speed)
with a maximum allowable deceleration over the block travel distance.
c. The last (or newest appended) block is planned from a complete stop (an exit speed of zero).
2. Go over every block in chronological (forward) order and dial down junction speed values if 2. Go over every block in chronological (forward) order and dial down junction speed values if
a. The speed increase within one block would require faster acceleration than the acceleration limits. a. The exit speed exceeds the one forward-computed from its entry speed with the maximum allowable
acceleration over the block travel distance.
When these stages are complete, all blocks have a junction entry speed that will allow all speed changes When these stages are complete, the planner will have maximized the velocity profiles throughout the all
to be performed using the overall limiting acceleration value, and where no junction speed is greater of the planner blocks, where every block is operating at its maximum allowable acceleration limits. In
than the max limit. In other words, it just computed the fastest possible velocity profile through all other words, for all of the blocks in the planner, the plan is optimal and no further speed improvements
buffered blocks, where the final buffered block is planned to come to a full stop when the buffer is fully are possible. If a new block is added to the buffer, the plan is recomputed according to the said
executed. Finally it will: guidelines for a new optimal plan.
3. Convert the plan to data that the stepper algorithm needs. Only block trapezoids adjacent to a To increase computational efficiency of these guidelines, a set of planner block pointers have been
a planner-modified junction speed with be updated, the others are assumed ok as is. created to indicate stop-compute points for when the planner guidelines cannot logically make any further
changes or improvements to the plan when in normal operation and new blocks are streamed and added to the
planner buffer. For example, if a subset of sequential blocks in the planner have been planned and are
bracketed by junction velocities at their maximums (or by the first planner block as well), no new block
added to the planner buffer will alter the velocity profiles within them. So we no longer have to compute
them. Or, if a set of sequential blocks from the first block in the planner (or a optimal stop-compute
point) are all accelerating, they are all optimal and can not be altered by a new block added to the
planner buffer, as this will only further increase the plan speed to chronological blocks until a maximum
junction velocity is reached. However, if the operational conditions of the plan changes from infrequently
used feed holds or feedrate overrides, the stop-compute pointers will be reset and the entire plan is
recomputed as stated in the general guidelines.
All planner computations(1)(2) are performed in floating point to minimize numerical round-off errors. Only Planner buffer index mapping:
when planned values are converted to stepper rate parameters(3), these are integers. If another motion block - block_buffer_head: Points to the newest incoming buffer block just added by plan_buffer_line(). The
is added while executing, the planner will re-plan and update the stored optimal velocity profile as it goes. planner never touches the exit speed of this block, which always defaults to 0.
Conceptually, the planner works like blowing up a balloon, where the balloon is the velocity profile. It's
constrained by the speeds at the beginning and end of the buffer, along with the maximum junction speeds and
nominal speeds of each block. Once a plan is computed, or balloon filled, this is the optimal velocity profile
through all of the motions in the buffer. Whenever a new block is added, this changes some of the limiting
conditions, or how the balloon is filled, so it has to be re-calculated to get the new optimal velocity profile.
Also, since the planner only computes on what's in the planner buffer, some motions with lots of short line
segments, like arcs, may seem to move slow. This is because there simply isn't enough combined distance traveled
in the entire buffer to accelerate up to the nominal speed and then decelerate to a stop at the end of the
buffer. There are a few simple solutions to this: (1) Maximize the machine acceleration. The planner will be
able to compute higher speed profiles within the same combined distance. (2) Increase line segment(s) distance.
The more combined distance the planner has to use, the faster it can go. (3) Increase the MINIMUM_JUNCTION_SPEED.
Not recommended. This will change what speed the planner plans to at the end of the buffer. Can lead to lost
steps when coming to a stop. (4) [BEST] Increase the planner buffer size. The more combined distance, the
bigger the balloon, or faster it can go. But this is not possible for 328p Arduinos because its limited memory
is already maxed out. Future ARM versions should not have this issue, with look-ahead planner blocks numbering
up to a hundred or more.
NOTE: Since this function is constantly re-calculating for every new incoming block, it must be as efficient
as possible. For example, in situations like arc generation or complex curves, the short, rapid line segments
can execute faster than new blocks can be added, and the planner buffer will then starve and empty, leading
to weird hiccup-like jerky motions.
Index mapping:
- block_buffer_head: Points to the newest incoming buffer block just added by plan_buffer_line(). The planner
never touches the exit speed of this block, which always defaults to MINIMUM_JUNCTION_SPEED.
- block_buffer_tail: Points to the beginning of the planner buffer. First to be executed or being executed. - block_buffer_tail: Points to the beginning of the planner buffer. First to be executed or being executed.
Can dynamically change with the old stepper algorithm, but with the new algorithm, this should be impossible Can dynamically change with the old stepper algorithm, but with the new algorithm, this should be impossible
as long as the segment buffer is not empty. as long as the segment buffer is not empty.
@ -193,71 +175,47 @@ void plan_update_partial_block(uint8_t block_index, float exit_speed_sqr)
entry speed. entry speed.
!!! Need to check if this is the start of the non-optimal or the end of the optimal block. !!! Need to check if this is the start of the non-optimal or the end of the optimal block.
NOTE: All planner computations are performed in floating point to minimize numerical round-off errors.
When a planner block is executed, the floating point values are converted to fast integers by the stepper
algorithm segment buffer. See the stepper module for details.
NOTE: Since the planner only computes on what's in the planner buffer, some motions with lots of short
line segments, like G2/3 arcs or complex curves, may seem to move slow. This is because there simply isn't
enough combined distance traveled in the entire buffer to accelerate up to the nominal speed and then
decelerate to a complete stop at the end of the buffer, as stated by the guidelines. If this happens and
becomes an annoyance, there are a few simple solutions: (1) Maximize the machine acceleration. The planner
will be able to compute higher velocity profiles within the same combined distance. (2) Maximize line
segment(s) distance per block to a desired tolerance. The more combined distance the planner has to use,
the faster it can go. (3) Maximize the planner buffer size. This also will increase the combined distance
for the planner to compute over. It also increases the number of computations the planner has to perform
to compute an optimal plan, so select carefully. The Arduino 328p memory is already maxed out, but future
ARM versions should have enough memory and speed for look-ahead blocks numbering up to a hundred or more.
*/ */
static void planner_recalculate() static void planner_recalculate()
{ {
// Initialize block index to the last block in the planner buffer.
uint8_t block_index = prev_block_index(block_buffer_head);
// Query stepper module for safe planner block index to recalculate to, which corresponds to the end // Query stepper module for safe planner block index to recalculate to, which corresponds to the end
// of the step segment buffer. // of the step segment buffer.
uint8_t block_buffer_safe = st_get_prep_block_index(); uint8_t block_buffer_safe = st_get_prep_block_index();
// TODO: Make sure that we don't have to check for the block_buffer_tail condition, if the stepper module // TODO: Make sure that we don't have to check for the block_buffer_tail condition, if the stepper module
// returns a NULL pointer or something. This could happen when the segment buffer is empty. Although, // returns a NULL pointer or something. This could happen when the segment buffer is empty. Although,
// this call won't return a NULL, only an index.. I have to make sure that this index is synced with the // this call won't return a NULL, only an index.. I have to make sure that this index is synced with the
// planner at all times. // planner at all times.
/* - In theory, the state of the segment buffer can exist anywhere within the planner buffer tail and head-1
or is empty, when there is nothing in the segment queue. The safe pointer can be the buffer head only
when the planner queue has been entirely queued into the segment buffer and there are no more blocks
in the planner buffer. The segment buffer will to continue to execute the remainder of it, but the
planner should be able to treat a newly added block during this time as an empty planner buffer since
we can't touch the segment buffer.
- The segment buffer is atomic to the planner buffer, because the main program computes these seperately.
Even if we move the planner head pointer early at the end of plan_buffer_line(), this shouldn't
effect the safe pointer.
- If the safe pointer is at head-1, this means that the stepper algorithm has segments queued and may
be executing. This is the last block in the planner queue, so it has been planned to decelerate to
zero at its end. When adding a new block, there will be at least two blocks to work with. When resuming,
from a feed hold, we only have this block and will be computing nothing. The planner doesn't have to
do anything, since the trapezoid calculations called by the stepper module should complete the block plan.
- In most cases, the safe pointer is at the plan tail or the block after, and rarely on the block two
beyond the tail. Since the safe pointer points to the block used at the end of the segment buffer, it
can be in any one of these states. As the stepper module executes the planner block, the buffer tail,
and hence the safe pointer, can push forward through the planner blocks and overcome the planned
pointer at any time.
- Does the reverse pass not touch either the safe or the plan pointer blocks? The plan pointer only
allows the velocity profile within it to be altered, but not the entry speed, so the reverse pass
ignores this block. The safe pointer is the same way, where the entry speed does not change, but
the velocity profile within it does.
- The planned pointer can exist anywhere in a given plan, except for the planner buffer head, if everything
operates as anticipated. Since the planner buffer can be executed by the stepper algorithm as any
rate and could empty the planner buffer quickly, the planner tail can overtake the planned pointer
at any time, but will never go around the ring buffer and re-encounter itself, the plan itself is not
changed by adding a new block or something else.
- The planner recalculate function should always reset the planned pointer at the proper break points
or when it encounters the safe block pointer, but will only do so when there are more than one block
in the buffer. In the case of single blocks, the planned pointer should always be set to the first
write-able block in the buffer, aka safe block.
- When does this not work? There might be an issue when the planned pointer moves from the tail to the
next head as a new block is being added and planned. Otherwise, the planned pointer should remain
static within the ring buffer no matter what the buffer is doing: being executed, adding new blocks,
or both simultaneously. Need to make sure that this case is covered.
*/
// Recompute plan only when there is more than one planner block in the buffer. Can't do anything with one. // Recompute plan only when there is more than one planner block in the buffer. Can't do anything with one.
// NOTE: block_buffer_safe can be equal to block_buffer_head if the segment buffer has completely queued up // NOTE: block_buffer_safe can be the last planner block if the segment buffer has completely queued up the
// the remainder of the planner buffer. In this case, a new planner block will be treated as a single block. // remainder of the planner buffer. In this case, a new planner block will be treated as a single block.
if (block_buffer_head == block_buffer_safe) { // Also catches head = tail if (block_index == block_buffer_safe) { // Also catches (head-1) = tail
// Just set block_buffer_planned pointer. // Just set block_buffer_planned pointer.
block_buffer_planned = block_buffer_head; block_buffer_planned = block_index;
printString("z");
// TODO: Feedrate override of one block needs to update the partial block with an exit speed of zero. For // TODO: Feedrate override of one block needs to update the partial block with an exit speed of zero. For
// a single added block and recalculate after a feed hold, we don't need to compute this, since we already // a single added block and recalculate after a feed hold, we don't need to compute this, since we already
@ -271,7 +229,6 @@ static void planner_recalculate()
// all junctions before proceeding. // all junctions before proceeding.
// Initialize planner buffer pointers and indexing. // Initialize planner buffer pointers and indexing.
uint8_t block_index = block_buffer_head;
plan_block_t *current = &block_buffer[block_index]; plan_block_t *current = &block_buffer[block_index];
// Calculate maximum entry speed for last block in buffer, where the exit speed is always zero. // Calculate maximum entry speed for last block in buffer, where the exit speed is always zero.
@ -285,7 +242,7 @@ static void planner_recalculate()
// will be recomputed within the plan. So, we need to update it if it is partially completed. // will be recomputed within the plan. So, we need to update it if it is partially completed.
float entry_speed_sqr; float entry_speed_sqr;
plan_block_t *next; plan_block_t *next;
block_index = prev_block_index(block_index); block_index = plan_prev_block_index(block_index);
if (block_index == block_buffer_safe) { // !! OR plan pointer? Yes I think so. if (block_index == block_buffer_safe) { // !! OR plan pointer? Yes I think so.
@ -294,7 +251,6 @@ static void planner_recalculate()
// !!! Need to make the current entry speed calculation after this. // !!! Need to make the current entry speed calculation after this.
plan_update_partial_block(block_index, 0.0); plan_update_partial_block(block_index, 0.0);
block_buffer_planned = block_index; block_buffer_planned = block_index;
printString("y");
} else { } else {
@ -306,7 +262,7 @@ printString("y");
// Increment block index early to check if the safe block is before the current block. If encountered, // Increment block index early to check if the safe block is before the current block. If encountered,
// this is an exit condition as we can't go further than this block in the reverse pass. // this is an exit condition as we can't go further than this block in the reverse pass.
block_index = prev_block_index(block_index); block_index = plan_prev_block_index(block_index);
if (block_index == block_buffer_safe) { if (block_index == block_buffer_safe) {
// Check if the safe block is partially completed. If so, update it before its exit speed // Check if the safe block is partially completed. If so, update it before its exit speed
// (=current->entry speed) is over-written. // (=current->entry speed) is over-written.
@ -314,7 +270,7 @@ printString("y");
// the previous nominal speed to update this block with. There will need to be something along the // the previous nominal speed to update this block with. There will need to be something along the
// lines of a nominal speed change check and send the correct value to this function. // lines of a nominal speed change check and send the correct value to this function.
plan_update_partial_block(block_index,current->entry_speed_sqr); plan_update_partial_block(block_index,current->entry_speed_sqr);
printString("x");
// Set planned pointer at safe block and for loop exit after following computation is done. // Set planned pointer at safe block and for loop exit after following computation is done.
block_buffer_planned = block_index; block_buffer_planned = block_index;
} }
@ -335,8 +291,8 @@ printString("x");
// Forward Pass: Forward plan the acceleration curve from the planned pointer onward. // Forward Pass: Forward plan the acceleration curve from the planned pointer onward.
// Also scans for optimal plan breakpoints and appropriately updates the planned pointer. // Also scans for optimal plan breakpoints and appropriately updates the planned pointer.
next = &block_buffer[block_buffer_planned]; // Begin at buffer planned pointer next = &block_buffer[block_buffer_planned]; // Begin at buffer planned pointer
block_index = next_block_index(block_buffer_planned); block_index = plan_next_block_index(block_buffer_planned);
while (block_index != next_buffer_head) { while (block_index != block_buffer_head) {
current = next; current = next;
next = &block_buffer[block_index]; next = &block_buffer[block_index];
@ -363,11 +319,63 @@ printString("x");
block_buffer_planned = block_index; // Set optimal plan pointer block_buffer_planned = block_index; // Set optimal plan pointer
} }
block_index = next_block_index( block_index ); block_index = plan_next_block_index( block_index );
} }
} }
/*
uint8_t block_buffer_safe = st_get_prep_block_index();
if (block_buffer_head == block_buffer_safe) { // Also catches head = tail
block_buffer_planned = block_buffer_head;
} else {
uint8_t block_index = block_buffer_head;
plan_block_t *current = &block_buffer[block_index];
current->entry_speed_sqr = min( current->max_entry_speed_sqr, 2*current->acceleration*current->millimeters);
float entry_speed_sqr;
plan_block_t *next;
block_index = plan_prev_block_index(block_index);
if (block_index == block_buffer_safe) { // !! OR plan pointer? Yes I think so.
plan_update_partial_block(block_index, 0.0);
block_buffer_planned = block_index;
} else {
while (block_index != block_buffer_planned) {
next = current;
current = &block_buffer[block_index];
block_index = plan_prev_block_index(block_index);
if (block_index == block_buffer_safe) {
plan_update_partial_block(block_index,current->entry_speed_sqr);
block_buffer_planned = block_index;
}
if (current->entry_speed_sqr != current->max_entry_speed_sqr) {
entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters;
if (entry_speed_sqr < current->max_entry_speed_sqr) {
current->entry_speed_sqr = entry_speed_sqr;
} else {
current->entry_speed_sqr = current->max_entry_speed_sqr;
}
}
}
}
next = &block_buffer[block_buffer_planned]; // Begin at buffer planned pointer
block_index = plan_next_block_index(block_buffer_planned);
while (block_index != next_buffer_head) {
current = next;
next = &block_buffer[block_index];
if (current->entry_speed_sqr < next->entry_speed_sqr) {
entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters;
if (entry_speed_sqr < next->entry_speed_sqr) {
next->entry_speed_sqr = entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass sets this.
block_buffer_planned = block_index; // Set optimal plan pointer.
}
}
if (next->entry_speed_sqr == next->max_entry_speed_sqr) {
block_buffer_planned = block_index; // Set optimal plan pointer
}
block_index = plan_next_block_index( block_index );
}
} */
} }
@ -376,11 +384,12 @@ void plan_reset_buffer()
block_buffer_planned = block_buffer_tail; block_buffer_planned = block_buffer_tail;
} }
void plan_init() void plan_init()
{ {
block_buffer_tail = 0; block_buffer_tail = 0;
block_buffer_head = 0; // Empty = tail block_buffer_head = 0; // Empty = tail
next_buffer_head = 1; // next_block_index(block_buffer_head) next_buffer_head = 1; // plan_next_block_index(block_buffer_head)
plan_reset_buffer(); plan_reset_buffer();
memset(&pl, 0, sizeof(pl)); // Clear planner struct memset(&pl, 0, sizeof(pl)); // Clear planner struct
} }
@ -389,7 +398,7 @@ void plan_init()
void plan_discard_current_block() void plan_discard_current_block()
{ {
if (block_buffer_head != block_buffer_tail) { // Discard non-empty buffer. if (block_buffer_head != block_buffer_tail) { // Discard non-empty buffer.
block_buffer_tail = next_block_index( block_buffer_tail ); block_buffer_tail = plan_next_block_index( block_buffer_tail );
} }
} }
@ -484,10 +493,8 @@ void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate)
if (feed_rate < 0) { feed_rate = SOME_LARGE_VALUE; } // Scaled down to absolute max/rapids rate later if (feed_rate < 0) { feed_rate = SOME_LARGE_VALUE; } // Scaled down to absolute max/rapids rate later
else if (invert_feed_rate) { feed_rate = block->millimeters/feed_rate; } else if (invert_feed_rate) { feed_rate = block->millimeters/feed_rate; }
// Calculate the unit vector of the line move and the block maximum feed rate and acceleration limited // Calculate the unit vector of the line move and the block maximum feed rate and acceleration scaled
// by the maximum possible values. Block rapids rates are computed or feed rates are scaled down so // down such that no individual axes maximum values are exceeded with respect to the line direction.
// they don't exceed the maximum axes velocities. The block acceleration is maximized based on direction
// and axes properties as well.
// NOTE: This calculation assumes all axes are orthogonal (Cartesian) and works with ABC-axes, // NOTE: This calculation assumes all axes are orthogonal (Cartesian) and works with ABC-axes,
// if they are also orthogonal/independent. Operates on the absolute value of the unit vector. // if they are also orthogonal/independent. Operates on the absolute value of the unit vector.
float inverse_unit_vec_value; float inverse_unit_vec_value;
@ -514,10 +521,6 @@ void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate)
if (block_buffer_head == block_buffer_tail) { if (block_buffer_head == block_buffer_tail) {
// Initialize block entry speed as zero. Assume it will be starting from rest. Planner will correct this later. // Initialize block entry speed as zero. Assume it will be starting from rest. Planner will correct this later.
// !!! Ensures when the first block starts from zero speed. If we do this in the planner, this will break
// feedrate overrides later, as you can override this single block and it maybe moving already at a given rate.
// Better to do it here and make it clean.
// !!! Shouldn't need this for anything other than a single block.
block->entry_speed_sqr = 0.0; block->entry_speed_sqr = 0.0;
block->max_junction_speed_sqr = 0.0; // Starting from rest. Enforce start from zero velocity. block->max_junction_speed_sqr = 0.0; // Starting from rest. Enforce start from zero velocity.
@ -541,10 +544,9 @@ void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate)
a continuous mode path, but ARM-based microcontrollers most certainly do. a continuous mode path, but ARM-based microcontrollers most certainly do.
NOTE: The max junction speed is a fixed value, since machine acceleration limits cannot be NOTE: The max junction speed is a fixed value, since machine acceleration limits cannot be
changed dynamically during operation nor can the line segment geometry. This must be kept in changed dynamically during operation nor can the line move geometry. This must be kept in
memory in the event of a feedrate override changing the nominal speeds of blocks, which can memory in the event of a feedrate override changing the nominal speeds of blocks, which can
change the overall maximum entry speed conditions of all blocks. change the overall maximum entry speed conditions of all blocks.
*/ */
// NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta). // NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta).
float sin_theta_d2 = sqrt(0.5*(1.0-junction_cos_theta)); // Trig half angle identity. Always positive. float sin_theta_d2 = sqrt(0.5*(1.0-junction_cos_theta)); // Trig half angle identity. Always positive.
@ -558,7 +560,6 @@ void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate)
block->nominal_speed_sqr = feed_rate*feed_rate; // (mm/min). Always > 0 block->nominal_speed_sqr = feed_rate*feed_rate; // (mm/min). Always > 0
// Compute the junction maximum entry based on the minimum of the junction speed and neighboring nominal speeds. // Compute the junction maximum entry based on the minimum of the junction speed and neighboring nominal speeds.
// TODO: Should call a function to determine this. The function can be used elsewhere for feedrate overrides later.
block->max_entry_speed_sqr = min(block->max_junction_speed_sqr, block->max_entry_speed_sqr = min(block->max_junction_speed_sqr,
min(block->nominal_speed_sqr,pl.previous_nominal_speed_sqr)); min(block->nominal_speed_sqr,pl.previous_nominal_speed_sqr));
@ -569,17 +570,16 @@ void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate)
// Update planner position // Update planner position
memcpy(pl.position, target_steps, sizeof(target_steps)); // pl.position[] = target_steps[] memcpy(pl.position, target_steps, sizeof(target_steps)); // pl.position[] = target_steps[]
planner_recalculate(); // New block is all set. Update buffer head and next buffer head indices.
// Update buffer head and next buffer head indices. Advance only after new plan has been computed.
block_buffer_head = next_buffer_head; block_buffer_head = next_buffer_head;
next_buffer_head = next_block_index(block_buffer_head); next_buffer_head = plan_next_block_index(block_buffer_head);
// Finish up by recalculating the plan with the new block.
planner_recalculate();
// int32_t blength = block_buffer_head - block_buffer_tail;
int32_t blength = block_buffer_head - block_buffer_tail; // if (blength < 0) { blength += BLOCK_BUFFER_SIZE; }
if (blength < 0) { blength += BLOCK_BUFFER_SIZE; } // printInteger(blength);
printInteger(blength);
} }
@ -595,36 +595,34 @@ void plan_sync_position()
} }
/* STEPPER VELOCITY PROFILE DEFINITION /* STEPPER VELOCITY PROFILE DEFINITION
less than nominal rate-> + less than nominal speed-> +
+--------+ <- nominal_rate /|\ +--------+ <- nominal_speed /|\
/ \ / | \ / \ / | \
initial_rate -> + \ / | + <- next->initial_rate entry_speed -> + \ / | + <- next->entry_speed
| + <- next->initial_rate / | | | + <- next->entry_speed / | |
+-------------+ initial_rate -> +----+--+ +-------------+ entry_speed -> +----+--+
time --> ^ ^ ^ ^ time --> ^ ^ ^ ^
| | | | | | | |
decelerate distance decelerate distance decelerate distance decelerate distance
Calculates the "trapezoid" velocity profile parameters of a planner block for the stepper Calculates the type of velocity profile for a given planner block and provides the deceleration
algorithm. The planner computes the entry and exit speeds of each block, but does not bother to distance for the stepper algorithm to use to accurately trace the profile exactly. The planner
determine the details of the velocity profiles within them, as they aren't needed for computing computes the entry and exit speeds of each block, but does not bother to determine the details of
an optimal plan. When the stepper algorithm begins to execute a block, the block velocity profiles the velocity profiles within them, as they aren't needed for computing an optimal plan. When the
are computed ad hoc. stepper algorithm begins to execute a block, the block velocity profiles are computed ad hoc.
Each block velocity profiles can be described as either a trapezoidal or a triangular shape. The Each block velocity profiles can be described as either a trapezoidal or a triangular shape. The
trapezoid occurs when the block reaches the nominal speed of the block and cruises for a period of trapezoid occurs when the block reaches the nominal speed of the block and cruises for a period of
time. A triangle occurs when the nominal speed is not reached within the block. Both of these time. A triangle occurs when the nominal speed is not reached within the block. Both of these
velocity profiles may also be truncated on either end with no acceleration or deceleration ramps, velocity profiles may also be truncated on either end with no acceleration or deceleration ramps,
as they can be influenced by the conditions of neighboring blocks. as they can be influenced by the conditions of neighboring blocks, where the acceleration ramps
are defined by constant acceleration equal to the maximum allowable acceleration of a block.
The following function determines the type of velocity profile and stores the minimum required Since the stepper algorithm already assumes to begin executing a planner block by accelerating
information for the stepper algorithm to execute the calculated profiles. Since the stepper from the planner entry speed and cruise if the nominal speed is reached, we only need to know
algorithm always assumes to begin accelerating from the initial_rate and cruise if the nominal_rate when to begin deceleration to the end of the block. Hence, only the distance from the end of the
is reached, we only need to know when to begin deceleration to the end of the block. Hence, only block to begin a deceleration ramp is computed for the stepper algorithm when requested.
the distance from the end of the block to begin a deceleration ramp are computed.
*/ */
float plan_calculate_velocity_profile(uint8_t block_index) float plan_calculate_velocity_profile(uint8_t block_index)
{ {
@ -632,7 +630,7 @@ float plan_calculate_velocity_profile(uint8_t block_index)
// Determine current block exit speed // Determine current block exit speed
float exit_speed_sqr = 0.0; // Initialize for end of planner buffer. Zero speed. float exit_speed_sqr = 0.0; // Initialize for end of planner buffer. Zero speed.
plan_block_t *next_block = plan_get_block_by_index(next_block_index(block_index)); plan_block_t *next_block = plan_get_block_by_index(plan_next_block_index(block_index));
if (next_block != NULL) { exit_speed_sqr = next_block->entry_speed_sqr; } // Exit speed is the entry speed of next buffer block if (next_block != NULL) { exit_speed_sqr = next_block->entry_speed_sqr; } // Exit speed is the entry speed of next buffer block
// First determine intersection distance (in steps) from the exit point for a triangular profile. // First determine intersection distance (in steps) from the exit point for a triangular profile.

2
planner.h
View File

@ -64,6 +64,8 @@ void plan_discard_current_block();
// Gets the current block. Returns NULL if buffer empty // Gets the current block. Returns NULL if buffer empty
plan_block_t *plan_get_current_block(); plan_block_t *plan_get_current_block();
uint8_t plan_next_block_index(uint8_t block_index);
plan_block_t *plan_get_block_by_index(uint8_t block_index); plan_block_t *plan_get_block_by_index(uint8_t block_index);
float plan_calculate_velocity_profile(uint8_t block_index); float plan_calculate_velocity_profile(uint8_t block_index);

112
stepper.c
View File

@ -115,23 +115,6 @@ static uint8_t st_data_prep_index; // Index of stepper common data block bein
static uint8_t pl_partial_block_flag; // Flag indicating the planner has modified the prepped planner block static uint8_t pl_partial_block_flag; // Flag indicating the planner has modified the prepped planner block
// Returns the index of the next block in the ring buffer
// NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication.
static uint8_t next_block_index(uint8_t block_index)
{
block_index++;
if (block_index == SEGMENT_BUFFER_SIZE) { block_index = 0; }
return(block_index);
}
static uint8_t next_block_pl_index(uint8_t block_index)
{
block_index++;
if (block_index == BLOCK_BUFFER_SIZE) { block_index = 0; }
return(block_index);
}
/* __________________________ /* __________________________
/| |\ _________________ ^ /| |\ _________________ ^
/ | | \ /| |\ | / | | \ /| |\ |
@ -334,9 +317,7 @@ ISR(TIMER2_COMPA_vect)
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter
if (st.delta_d > st_current_data->rate_delta) { if (st.delta_d > st_current_data->rate_delta) {
st.delta_d -= st_current_data->rate_delta; st.delta_d -= st_current_data->rate_delta;
} else { } else { // Moving near zero feed rate. Gracefully slow down.
// Moving near zero feed rate. Gracefully slow down.
st.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow. st.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow.
// TODO: Check for and handle feed hold exit? At this point, machine is stopped. // TODO: Check for and handle feed hold exit? At this point, machine is stopped.
@ -392,16 +373,16 @@ ISR(TIMER2_COMPA_vect)
// Check step events for trapezoid change or end of block. // Check step events for trapezoid change or end of block.
st.segment_steps_remaining--; // Decrement step events count st.segment_steps_remaining--; // Decrement step events count
if (st.segment_steps_remaining == 0) { if (st.segment_steps_remaining == 0) {
// NOTE: sys.position updates could be done here. The bresenham counters can have
// their own fast 8-bit addition-only counters. Here we would check the direction and
// apply it to sys.position accordingly. However, this could take too much time
// combined with loading a new segment during next cycle too.
// TODO: Measure the time it would take in the worst case. It could still be faster
// overall during segment execution if uint8 step counters tracked this and was added
// to the system position variables here. Compared to worst case now, it wouldn't be
// that much different.
/* /*
NOTE: sys.position updates could be done here. The bresenham counters can have
their own fast 8-bit addition-only counters. Here we would check the direction and
apply it to sys.position accordingly. However, this could take too much time
combined with loading a new segment during next cycle too.
TODO: Measure the time it would take in the worst case. It could still be faster
overall during segment execution if uint8 step counters tracked this and was added
to the system position variables here. Compared to worst case now, it wouldn't be
that much different.
// TODO: Upon loading, step counters would need to be zeroed. // TODO: Upon loading, step counters would need to be zeroed.
// TODO: For feedrate overrides, we will have to execute add these values.. although // TODO: For feedrate overrides, we will have to execute add these values.. although
// for probing, this breaks. Current values won't be correct, unless we query it. // for probing, this breaks. Current values won't be correct, unless we query it.
@ -429,8 +410,8 @@ ISR(TIMER2_COMPA_vect)
st.load_flag = LOAD_SEGMENT; st.load_flag = LOAD_SEGMENT;
} }
// Discard current segment // Discard current segment by advancing buffer tail index
segment_buffer_tail = next_block_index( segment_buffer_tail ); if ( ++segment_buffer_tail == SEGMENT_BUFFER_SIZE) { segment_buffer_tail = 0; }
} }
@ -552,28 +533,32 @@ void st_cycle_reinitialize()
/* Prepares step segment buffer. Continuously called from main program. /* Prepares step segment buffer. Continuously called from main program.
NOTE: There doesn't seem to be a great way to figure out how many steps occur within The segment buffer is an intermediary buffer interface between the execution of steps
a set number of ISR ticks. Numerical round-off and CPU overhead always seems to be a by the stepper algorithm and the velocity profiles generated by the planner. The stepper
critical problem. So, either numerical round-off checks could be made to account for algorithm only executes steps within the segment buffer and is filled by the main program
them, while CPU overhead could be minimized in some way, or we can flip the algorithm when steps are "checked-out" from the first block in the planner buffer. This keeps the
around to have the stepper algorithm track number of steps over an indeterminant amount step execution and planning optimization processes atomic and protected from each other.
of time instead. The number of steps "checked-out" from the planner buffer and the number of segments in
In other words, we use the planner velocity floating point data to get an estimate of the segment buffer is sized and computed such that no operation in the main program takes
the number of steps we want to execute. We then back out the approximate velocity for longer than the time it takes the stepper algorithm to empty it before refilling it.
the planner to use, which should be much more robust to round-off error. The main problem Currently, the segment buffer conservatively holds roughly up to 40-60 msec of steps.
now is that we are loading the stepper algorithm to handle acceleration now, rather than
pre-calculating with the main program. This approach does make sense in the way that
planner velocities and stepper profiles can be traced more accurately.
Which is better? Very hard to tell. The time-based algorithm would be able to handle
Bresenham step adaptive-resolution much easier and cleaner. Whereas, the step-based would
require some additional math in the stepper algorithm to adjust on the fly, plus adaptation
would occur in a non-deterministic manner.
I suppose it wouldn't hurt to build both to see what's better. Just a lot more work.
TODO: Need to describe the importance of continuations of step pulses between ramp states NOTE: The segment buffer executes a set number of steps over an approximate time period.
and planner blocks. This has to do with Alden's problem with step "phase". The things I've If we try to execute over a set time period, it is difficult to guarantee or predict how
been doing here limit this phase issue by truncating some of the ramp timing for certain many steps will execute over it, especially when the step pulse phasing between the
events like deceleration initialization and end of block. neighboring segments are kept consistent. Meaning that, if the last segment step pulses
right before its end, the next segment must delay its first pulse so that the step pulses
are consistently spaced apart over time to keep the step pulse train nice and smooth.
Keeping track of phasing and ensuring that the exact number of steps are executed as
defined by the planner block, the related computational overhead gets quickly and
prohibitively expensive, especially in real-time.
Since the stepper algorithm automatically takes care of the step pulse phasing with
its ramp and inverse time counters, we don't have to explicitly and expensively track the
exact number of steps, time, or phasing of steps. All we need to do is approximate
the number of steps in each segment such that the segment buffer has enough execution time
for the main program to do what it needs to do and refill it when it has time. In other
words, we just need to compute a cheap approximation of the current velocity and the
number of steps over it.
*/ */
/* /*
@ -606,7 +591,7 @@ void st_prep_buffer()
pl_prep_block = plan_get_block_by_index(pl_prep_index); // Query planner for a queued block pl_prep_block = plan_get_block_by_index(pl_prep_index); // Query planner for a queued block
if (pl_prep_block == NULL) { return; } // No planner blocks. Exit. if (pl_prep_block == NULL) { return; } // No planner blocks. Exit.
// Increment stepper common data buffer index // Increment stepper common data index
if ( ++st_data_prep_index == (SEGMENT_BUFFER_SIZE-1) ) { st_data_prep_index = 0; } if ( ++st_data_prep_index == (SEGMENT_BUFFER_SIZE-1) ) { st_data_prep_index = 0; }
// Check if the planner has re-computed this block mid-execution. If so, push the previous segment // Check if the planner has re-computed this block mid-execution. If so, push the previous segment
@ -666,7 +651,10 @@ void st_prep_buffer()
// Set new segment to point to the current segment data block. // Set new segment to point to the current segment data block.
prep_segment->st_data_index = st_data_prep_index; prep_segment->st_data_index = st_data_prep_index;
// Approximate the velocity over the new segment // Approximate the velocity over the new segment using the already computed rate values.
// NOTE: This assumes that each segment will have an execution time roughly equal to every ACCELERATION_TICK.
// We do this to minimize memory and computational requirements. However, this could easily be replaced with
// a more exact approximation or have a unique time per segment, if CPU and memory overhead allows.
if (st_prep_data->decelerate_after <= 0) { if (st_prep_data->decelerate_after <= 0) {
if (st_prep_data->decelerate_after == 0) { prep_segment->flag = SEGMENT_DECEL; } if (st_prep_data->decelerate_after == 0) { prep_segment->flag = SEGMENT_DECEL; }
else { st_prep_data->current_approx_rate -= st_prep_data->rate_delta; } else { st_prep_data->current_approx_rate -= st_prep_data->rate_delta; }
@ -680,15 +668,15 @@ void st_prep_buffer()
} }
} }
// Compute the number of steps in the prepped segment based on the approximate current rate. The execution // Compute the number of steps in the prepped segment based on the approximate current rate.
// time of each segment should be about every ACCELERATION_TICK.
// NOTE: The d_next divide cancels out the INV_TIME_MULTIPLIER and converts the rate value to steps. // NOTE: The d_next divide cancels out the INV_TIME_MULTIPLIER and converts the rate value to steps.
// NOTE: As long as the ACCELERATION_TICKS_PER_SECOND is valid, n_step should never exceed 255.
prep_segment->n_step = ceil(max(MINIMUM_STEP_RATE,st_prep_data->current_approx_rate)* prep_segment->n_step = ceil(max(MINIMUM_STEP_RATE,st_prep_data->current_approx_rate)*
(ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND)/st_prep_data->d_next); (ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND)/st_prep_data->d_next);
prep_segment->n_step = max(prep_segment->n_step,MINIMUM_STEPS_PER_SEGMENT); // Ensure it moves for very slow motions? // NOTE: Ensures it moves for very slow motions, but MINIMUM_STEP_RATE should always set this too. Perhaps
// a compile-time check to see if MINIMUM_STEP_RATE is set high enough is all that is needed.
prep_segment->n_step = max(prep_segment->n_step,MINIMUM_STEPS_PER_SEGMENT);
// NOTE: As long as the ACCELERATION_TICKS_PER_SECOND is valid, n_step should never exceed 255 and overflow.
// prep_segment->n_step = min(prep_segment->n_step,MAXIMUM_STEPS_PER_BLOCK); // Prevent unsigned int8 overflow. // prep_segment->n_step = min(prep_segment->n_step,MAXIMUM_STEPS_PER_BLOCK); // Prevent unsigned int8 overflow.
// Check if n_step exceeds steps remaining in planner block. If so, truncate. // Check if n_step exceeds steps remaining in planner block. If so, truncate.
if (prep_segment->n_step > st_prep_data->step_events_remaining) { if (prep_segment->n_step > st_prep_data->step_events_remaining) {
@ -703,7 +691,7 @@ void st_prep_buffer()
} }
} }
// Update stepper block variables. // Update stepper common data variables.
st_prep_data->decelerate_after -= prep_segment->n_step; st_prep_data->decelerate_after -= prep_segment->n_step;
st_prep_data->step_events_remaining -= prep_segment->n_step; st_prep_data->step_events_remaining -= prep_segment->n_step;
@ -712,13 +700,13 @@ void st_prep_buffer()
// Set EOB bitflag so stepper algorithm discards the planner block after this segment completes. // Set EOB bitflag so stepper algorithm discards the planner block after this segment completes.
prep_segment->flag |= SEGMENT_END_OF_BLOCK; prep_segment->flag |= SEGMENT_END_OF_BLOCK;
// Move planner pointer to next block and flag to load a new block for the next segment. // Move planner pointer to next block and flag to load a new block for the next segment.
pl_prep_index = next_block_pl_index(pl_prep_index); pl_prep_index = plan_next_block_index(pl_prep_index);
pl_prep_block = NULL; pl_prep_block = NULL;
} }
// New step segment completed. Increment segment buffer indices. // New step segment completed. Increment segment buffer indices.
segment_buffer_head = segment_next_head; segment_buffer_head = segment_next_head;
segment_next_head = next_block_index(segment_buffer_head); if ( ++segment_next_head == SEGMENT_BUFFER_SIZE ) { segment_next_head = 0; }
// long a = prep_segment->n_step; // long a = prep_segment->n_step;
// printInteger(a); // printInteger(a);