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

Push old dev_2 draft to work on other things.

Browse Source 
- **NON-FUNCTIONAL**
- Contains an old draft of separating the stepper driver direct access
to the planner buffer. This is designed to keep the stepper and planner
modules independent and prevent overwriting or other complications. In
this way, feedrate override should be able to be installed as well.
- A number of planner optimizations are installed too.
- Not sure where the bugs are. Either in the new planner optimizations,
new stepper module updates, or in both. Or it just could be that the
Arduino AVR is choking with the new things it has to do.
This commit is contained in:
Sonny Jeon 2013-08-19 09:24:22 -06:00
parent 1fa3dad206
commit 7a175bd2db
23 changed files with 3160 additions and 704 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
config.h
View File

@ -25,7 +25,7 @@
// IMPORTANT: Any changes here requires a full re-compiling of the source code to propagate them.
// Default settings. Used when resetting EEPROM. Change to desired name in defaults.h
#define DEFAULTS_GENERIC
#define DEFAULTS_ZEN_TOOLWORKS_7x7
// Serial baud rate
#define BAUD_RATE 9600
@ -113,7 +113,7 @@
// interrupt of the current stepper driver algorithm theoretically up to a frequency of 35-40kHz, but
// CPU overhead increases exponentially as this frequency goes up. So there will be little left for
// other processes like arcs.
#define ISR_TICKS_PER_SECOND 30000L // Integer (Hz)
#define ISR_TICKS_PER_SECOND 20000L // Integer (Hz)
// The temporal resolution of the acceleration management subsystem. Higher number give smoother
// acceleration but may impact performance. If you run at very high feedrates (>15kHz or so) and
@ -121,7 +121,8 @@
// profiles and how the stepper program actually performs them. The correct value for this parameter
// is machine dependent, so it's advised to set this only as high as needed. Approximate successful
// values can widely range from 50 to 200 or more. Cannot be greater than ISR_TICKS_PER_SECOND/2.
#define ACCELERATION_TICKS_PER_SECOND 120L
// NOTE: Ramp count variable type in stepper module may need to be updated if changed.
#define ACCELERATION_TICKS_PER_SECOND 100L
// NOTE: Make sure this value is less than 256, when adjusting both dependent parameters.
#define ISR_TICKS_PER_ACCELERATION_TICK (ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND)
@ -134,7 +135,7 @@
// applications, the following multiplier value will work more than well enough. If you do have
// happened to weird stepper motion issues, try modifying this value by adding or subtracting a
// zero and report it to the Grbl administrators.
#define RANADE_MULTIPLIER 100000000.0
#define INV_TIME_MULTIPLIER 10000000.0
// Minimum planner junction speed. Sets the default minimum speed the planner plans for at the end
// of the buffer and all stops. This should not be much greater than zero and should only be changed

270
gcode.c
View File

@ -39,7 +39,9 @@ parser_state_t gc;
#define FAIL(status) gc.status_code = status;
static int next_statement(char *letter, float *float_ptr, char *line, uint8_t *char_counter);
static uint8_t next_statement(char *letter, float *float_ptr, char *line, uint8_t *char_counter);
static void gc_convert_arc_radius_mode(float *target) __attribute__((noinline));
static void select_plane(uint8_t axis_0, uint8_t axis_1, uint8_t axis_2)
{
@ -48,6 +50,7 @@ static void select_plane(uint8_t axis_0, uint8_t axis_1, uint8_t axis_2)
gc.plane_axis_2 = axis_2;
}
void gc_init()
{
memset(&gc, 0, sizeof(gc));
@ -61,20 +64,24 @@ void gc_init()
}
}
// Sets g-code parser position in mm. Input in steps. Called by the system abort and hard
// limit pull-off routines.
void gc_set_current_position(int32_t x, int32_t y, int32_t z)
void gc_sync_position(int32_t x, int32_t y, int32_t z)
{
gc.position[X_AXIS] = x/settings.steps_per_mm[X_AXIS];
gc.position[Y_AXIS] = y/settings.steps_per_mm[Y_AXIS];
gc.position[Z_AXIS] = z/settings.steps_per_mm[Z_AXIS];
uint8_t i;
for (i=0; i<N_AXIS; i++) {
gc.position[i] = sys.position[i]/settings.steps_per_mm[i];
}
}
static float to_millimeters(float value)
{
return(gc.inches_mode ? (value * MM_PER_INCH) : value);
}
// Executes one line of 0-terminated G-Code. The line is assumed to contain only uppercase
// characters and signed floating point values (no whitespace). Comments and block delete
// characters have been removed. In this function, all units and positions are converted and
@ -99,10 +106,12 @@ uint8_t gc_execute_line(char *line)
uint8_t absolute_override = false; // true(1) = absolute motion for this block only {G53}
uint8_t non_modal_action = NON_MODAL_NONE; // Tracks the actions of modal group 0 (non-modal)
float target[N_AXIS], offset[N_AXIS];
float target[N_AXIS];
clear_vector(target); // XYZ(ABC) axes parameters.
clear_vector(offset); // IJK Arc offsets are incremental. Value of zero indicates no change.
gc.arc_radius = 0;
clear_vector(gc.arc_offset); // IJK Arc offsets are incremental. Value of zero indicates no change.
gc.status_code = STATUS_OK;
/* Pass 1: Commands and set all modes. Check for modal group violations.
@ -208,7 +217,7 @@ uint8_t gc_execute_line(char *line)
NOTE: Grbl unconventionally pre-converts these parameter values based on the block G and M
commands. This is set out of the order of execution defined by NIST only for code efficiency/size
purposes, but should not affect proper g-code execution. */
float p = 0, r = 0;
float p = 0;
uint8_t l = 0;
char_counter = 0;
while(next_statement(&letter, &value, line, &char_counter)) {
@ -222,10 +231,10 @@ uint8_t gc_execute_line(char *line)
gc.feed_rate = to_millimeters(value); // millimeters per minute
}
break;
case 'I': case 'J': case 'K': offset[letter-'I'] = to_millimeters(value); break;
case 'I': case 'J': case 'K': gc.arc_offset[letter-'I'] = to_millimeters(value); break;
case 'L': l = trunc(value); break;
case 'P': p = value; break;
case 'R': r = to_millimeters(value); break;
case 'R': gc.arc_radius = to_millimeters(value); break;
case 'S':
if (value < 0) { FAIL(STATUS_INVALID_STATEMENT); } // Cannot be negative
// TBD: Spindle speed not supported due to PWM issues, but may come back once resolved.
@ -245,7 +254,8 @@ uint8_t gc_execute_line(char *line)
// If there were any errors parsing this line, we will return right away with the bad news
if (gc.status_code) { return(gc.status_code); }
uint8_t i;
// Initialize axis index
uint8_t idx;
/* Execute Commands: Perform by order of execution defined in NIST RS274-NGC.v3, Table 8, pg.41. */
@ -292,12 +302,12 @@ uint8_t gc_execute_line(char *line)
float coord_data[N_AXIS];
if (!settings_read_coord_data(int_value,coord_data)) { return(STATUS_SETTING_READ_FAIL); }
// Update axes defined only in block. Always in machine coordinates. Can change non-active system.
for (i=0; i<N_AXIS; i++) { // Axes indices are consistent, so loop may be used.
if (bit_istrue(axis_words,bit(i)) ) {
for (idx=0; idx<N_AXIS; idx++) { // Axes indices are consistent, so loop may be used.
if (bit_istrue(axis_words,bit(idx)) ) {
if (l == 20) {
coord_data[i] = gc.position[i]-target[i]; // L20: Update axis current position to target
coord_data[idx] = gc.position[idx]-target[idx]; // L20: Update axis current position to target
} else {
coord_data[i] = target[i]; // L2: Update coordinate system axis
coord_data[idx] = target[idx]; // L2: Update coordinate system axis
}
}
}
@ -312,15 +322,15 @@ uint8_t gc_execute_line(char *line)
// and absolute and incremental modes.
if (axis_words) {
// Apply absolute mode coordinate offsets or incremental mode offsets.
for (i=0; i<N_AXIS; i++) { // Axes indices are consistent, so loop may be used.
if ( bit_istrue(axis_words,bit(i)) ) {
for (idx=0; idx<N_AXIS; idx++) { // Axes indices are consistent, so loop may be used.
if ( bit_istrue(axis_words,bit(idx)) ) {
if (gc.absolute_mode) {
target[i] += gc.coord_system[i] + gc.coord_offset[i];
target[idx] += gc.coord_system[idx] + gc.coord_offset[idx];
} else {
target[i] += gc.position[i];
target[idx] += gc.position[idx];
}
} else {
target[i] = gc.position[i];
target[idx] = gc.position[idx];
}
}
mc_line(target, -1.0, false);
@ -349,9 +359,9 @@ uint8_t gc_execute_line(char *line)
} else {
// Update axes defined only in block. Offsets current system to defined value. Does not update when
// active coordinate system is selected, but is still active unless G92.1 disables it.
for (i=0; i<N_AXIS; i++) { // Axes indices are consistent, so loop may be used.
if (bit_istrue(axis_words,bit(i)) ) {
gc.coord_offset[i] = gc.position[i]-gc.coord_system[i]-target[i];
for (idx=0; idx<N_AXIS; idx++) { // Axes indices are consistent, so loop may be used.
if (bit_istrue(axis_words,bit(idx)) ) {
gc.coord_offset[idx] = gc.position[idx]-gc.coord_system[idx]-target[idx];
}
}
}
@ -383,17 +393,17 @@ uint8_t gc_execute_line(char *line)
// Convert all target position data to machine coordinates for executing motion. Apply
// absolute mode coordinate offsets or incremental mode offsets.
// NOTE: Tool offsets may be appended to these conversions when/if this feature is added.
for (i=0; i<N_AXIS; i++) { // Axes indices are consistent, so loop may be used to save flash space.
if ( bit_istrue(axis_words,bit(i)) ) {
for (idx=0; idx<N_AXIS; idx++) { // Axes indices are consistent, so loop may be used to save flash space.
if ( bit_istrue(axis_words,bit(idx)) ) {
if (!absolute_override) { // Do not update target in absolute override mode
if (gc.absolute_mode) {
target[i] += gc.coord_system[i] + gc.coord_offset[i]; // Absolute mode
target[idx] += gc.coord_system[idx] + gc.coord_offset[idx]; // Absolute mode
} else {
target[i] += gc.position[i]; // Incremental mode
target[idx] += gc.position[idx]; // Incremental mode
}
}
} else {
target[i] = gc.position[i]; // No axis word in block. Keep same axis position.
target[idx] = gc.position[idx]; // No axis word in block. Keep same axis position.
}
}
@ -417,105 +427,15 @@ uint8_t gc_execute_line(char *line)
// Check if at least one of the axes of the selected plane has been specified. If in center
// format arc mode, also check for at least one of the IJK axes of the selected plane was sent.
if ( !( bit_false(axis_words,bit(gc.plane_axis_2)) ) ||
( !r && !offset[gc.plane_axis_0] && !offset[gc.plane_axis_1] ) ) {
( !gc.arc_radius && !gc.arc_offset[gc.plane_axis_0] && !gc.arc_offset[gc.plane_axis_1] ) ) {
FAIL(STATUS_INVALID_STATEMENT);
} else {
if (r != 0) { // Arc Radius Mode
/*
We need to calculate the center of the circle that has the designated radius and passes
through both the current position and the target position. This method calculates the following
set of equations where [x,y] is the vector from current to target position, d == magnitude of
that vector, h == hypotenuse of the triangle formed by the radius of the circle, the distance to
the center of the travel vector. A vector perpendicular to the travel vector [-y,x] is scaled to the
length of h [-y/d*h, x/d*h] and added to the center of the travel vector [x/2,y/2] to form the new point
[i,j] at [x/2-y/d*h, y/2+x/d*h] which will be the center of our arc.
d^2 == x^2 + y^2
h^2 == r^2 - (d/2)^2
i == x/2 - y/d*h
j == y/2 + x/d*h
O <- [i,j]
- |
r - |
- |
- | h
- |
[0,0] -> C -----------------+--------------- T <- [x,y]
| <------ d/2 ---->|
C - Current position
T - Target position
O - center of circle that pass through both C and T
d - distance from C to T
r - designated radius
h - distance from center of CT to O
Expanding the equations:
d -> sqrt(x^2 + y^2)
h -> sqrt(4 * r^2 - x^2 - y^2)/2
i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
Which can be written:
i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
Which we for size and speed reasons optimize to:
h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2)
i = (x - (y * h_x2_div_d))/2
j = (y + (x * h_x2_div_d))/2
*/
// Calculate the change in position along each selected axis
float x = target[gc.plane_axis_0]-gc.position[gc.plane_axis_0];
float y = target[gc.plane_axis_1]-gc.position[gc.plane_axis_1];
clear_vector(offset);
// First, use h_x2_div_d to compute 4*h^2 to check if it is negative or r is smaller
// than d. If so, the sqrt of a negative number is complex and error out.
float h_x2_div_d = 4 * r*r - x*x - y*y;
if (h_x2_div_d < 0) { FAIL(STATUS_ARC_RADIUS_ERROR); return(gc.status_code); }
// Finish computing h_x2_div_d.
h_x2_div_d = -sqrt(h_x2_div_d)/hypot(x,y); // == -(h * 2 / d)
// Invert the sign of h_x2_div_d if the circle is counter clockwise (see sketch below)
if (gc.motion_mode == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d; }
/* The counter clockwise circle lies to the left of the target direction. When offset is positive,
the left hand circle will be generated - when it is negative the right hand circle is generated.
T <-- Target position
^
Clockwise circles with this center | Clockwise circles with this center will have
will have > 180 deg of angular travel | < 180 deg of angular travel, which is a good thing!
\ | /
center of arc when h_x2_div_d is positive -> x <----- | -----> x <- center of arc when h_x2_div_d is negative
|
|
C <-- Current position */
// Negative R is g-code-alese for "I want a circle with more than 180 degrees of travel" (go figure!),
// even though it is advised against ever generating such circles in a single line of g-code. By
// inverting the sign of h_x2_div_d the center of the circles is placed on the opposite side of the line of
// travel and thus we get the unadvisably long arcs as prescribed.
if (r < 0) {
h_x2_div_d = -h_x2_div_d;
r = -r; // Finished with r. Set to positive for mc_arc
}
// Complete the operation by calculating the actual center of the arc
offset[gc.plane_axis_0] = 0.5*(x-(y*h_x2_div_d));
offset[gc.plane_axis_1] = 0.5*(y+(x*h_x2_div_d));
if (gc.arc_radius != 0) { // Arc Radius Mode
// Compute arc radius and offsets
gc_convert_arc_radius_mode(target);
if (gc.status_code) { return(gc.status_code); }
} else { // Arc Center Format Offset Mode
r = hypot(offset[gc.plane_axis_0], offset[gc.plane_axis_1]); // Compute arc radius for mc_arc
gc.arc_radius = hypot(gc.arc_offset[gc.plane_axis_0], gc.arc_offset[gc.plane_axis_1]); // Compute arc radius for mc_arc
}
// Set clockwise/counter-clockwise sign for mc_arc computations
@ -523,9 +443,9 @@ uint8_t gc_execute_line(char *line)
if (gc.motion_mode == MOTION_MODE_CW_ARC) { isclockwise = true; }
// Trace the arc
mc_arc(gc.position, target, offset, gc.plane_axis_0, gc.plane_axis_1, gc.plane_axis_2,
mc_arc(gc.position, target, gc.arc_offset, gc.plane_axis_0, gc.plane_axis_1, gc.plane_axis_2,
(gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode,
r, isclockwise);
gc.arc_radius, isclockwise);
}
break;
}
@ -557,7 +477,7 @@ uint8_t gc_execute_line(char *line)
// Parses the next statement and leaves the counter on the first character following
// the statement. Returns 1 if there was a statements, 0 if end of string was reached
// or there was an error (check state.status_code).
static int next_statement(char *letter, float *float_ptr, char *line, uint8_t *char_counter)
static uint8_t next_statement(char *letter, float *float_ptr, char *line, uint8_t *char_counter)
{
if (line[*char_counter] == 0) {
return(0); // No more statements
@ -576,6 +496,100 @@ static int next_statement(char *letter, float *float_ptr, char *line, uint8_t *c
return(1);
}
static void gc_convert_arc_radius_mode(float *target)
{
/* We need to calculate the center of the circle that has the designated radius and passes
through both the current position and the target position. This method calculates the following
set of equations where [x,y] is the vector from current to target position, d == magnitude of
that vector, h == hypotenuse of the triangle formed by the radius of the circle, the distance to
the center of the travel vector. A vector perpendicular to the travel vector [-y,x] is scaled to the
length of h [-y/d*h, x/d*h] and added to the center of the travel vector [x/2,y/2] to form the new point
[i,j] at [x/2-y/d*h, y/2+x/d*h] which will be the center of our arc.
d^2 == x^2 + y^2
h^2 == r^2 - (d/2)^2
i == x/2 - y/d*h
j == y/2 + x/d*h
O <- [i,j]
- |
r - |
- |
- | h
- |
[0,0] -> C -----------------+--------------- T <- [x,y]
| <------ d/2 ---->|
C - Current position
T - Target position
O - center of circle that pass through both C and T
d - distance from C to T
r - designated radius
h - distance from center of CT to O
Expanding the equations:
d -> sqrt(x^2 + y^2)
h -> sqrt(4 * r^2 - x^2 - y^2)/2
i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
Which can be written:
i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
Which we for size and speed reasons optimize to:
h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2)
i = (x - (y * h_x2_div_d))/2
j = (y + (x * h_x2_div_d))/2 */
// Calculate the change in position along each selected axis
float x = target[gc.plane_axis_0]-gc.position[gc.plane_axis_0];
float y = target[gc.plane_axis_1]-gc.position[gc.plane_axis_1];
clear_vector(gc.arc_offset);
// First, use h_x2_div_d to compute 4*h^2 to check if it is negative or r is smaller
// than d. If so, the sqrt of a negative number is complex and error out.
float h_x2_div_d = 4 * gc.arc_radius*gc.arc_radius - x*x - y*y;
if (h_x2_div_d < 0) { FAIL(STATUS_ARC_RADIUS_ERROR); return; }
// Finish computing h_x2_div_d.
h_x2_div_d = -sqrt(h_x2_div_d)/hypot(x,y); // == -(h * 2 / d)
// Invert the sign of h_x2_div_d if the circle is counter clockwise (see sketch below)
if (gc.motion_mode == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d; }
/* The counter clockwise circle lies to the left of the target direction. When offset is positive,
the left hand circle will be generated - when it is negative the right hand circle is generated.
T <-- Target position
^
Clockwise circles with this center | Clockwise circles with this center will have
will have > 180 deg of angular travel | < 180 deg of angular travel, which is a good thing!
\ | /
center of arc when h_x2_div_d is positive -> x <----- | -----> x <- center of arc when h_x2_div_d is negative
|
|
C <-- Current position */
// Negative R is g-code-alese for "I want a circle with more than 180 degrees of travel" (go figure!),
// even though it is advised against ever generating such circles in a single line of g-code. By
// inverting the sign of h_x2_div_d the center of the circles is placed on the opposite side of the line of
// travel and thus we get the unadvisably long arcs as prescribed.
if (gc.arc_radius < 0) {
h_x2_div_d = -h_x2_div_d;
gc.arc_radius = -gc.arc_radius; // Finished with r. Set to positive for mc_arc
}
// Complete the operation by calculating the actual center of the arc
gc.arc_offset[gc.plane_axis_0] = 0.5*(x-(y*h_x2_div_d));
gc.arc_offset[gc.plane_axis_1] = 0.5*(y+(x*h_x2_div_d));
}
/*
Not supported:

8
gcode.h
View File

@ -82,7 +82,11 @@ typedef struct {
float coord_system[N_AXIS]; // Current work coordinate system (G54+). Stores offset from absolute machine
// position in mm. Loaded from EEPROM when called.
float coord_offset[N_AXIS]; // Retains the G92 coordinate offset (work coordinates) relative to
// machine zero in mm. Non-persistent. Cleared upon reset and boot.
// machine zero in mm. Non-persistent. Cleared upon reset and boot.
float arc_radius;
float arc_offset[N_AXIS];
} parser_state_t;
extern parser_state_t gc;
@ -93,6 +97,6 @@ void gc_init();
uint8_t gc_execute_line(char *line);
// Set g-code parser position. Input in steps.
void gc_set_current_position(int32_t x, int32_t y, int32_t z);
void gc_sync_position();
#endif

42
limits.c
View File

@ -96,16 +96,17 @@ static void homing_cycle(uint8_t cycle_mask, int8_t pos_dir, bool invert_pin, fl
// and speedy homing routine.
// NOTE: For each axes enabled, the following calculations assume they physically move
// an equal distance over each time step until they hit a limit switch, aka dogleg.
uint32_t step_event_count, steps[N_AXIS];
uint32_t step_event_count = 0;
uint8_t i, dist = 0;
uint32_t steps[N_AXIS];
clear_vector(steps);
for (i=0; i<N_AXIS; i++) {
if (cycle_mask & (1<<i)) {
dist++;
steps[i] = lround(settings.steps_per_mm[i]);
step_event_count = max(step_event_count,steps[i]);
}
}
step_event_count = max(steps[X_AXIS], max(steps[Y_AXIS], steps[Z_AXIS]));
// To ensure global acceleration is not exceeded, reduce the governing axes nominal rate
// by adjusting the actual axes distance traveled per step. This is the same procedure
@ -244,23 +245,26 @@ void limits_go_home()
// and the workspace volume is in all negative space.
void limits_soft_check(float *target)
{
if ( target[X_AXIS] > 0 || target[X_AXIS] < -settings.max_travel[X_AXIS] ||
target[Y_AXIS] > 0 || target[Y_AXIS] < -settings.max_travel[Y_AXIS] ||
target[Z_AXIS] > 0 || target[Z_AXIS] < -settings.max_travel[Z_AXIS] ) {
// Force feed hold if cycle is active. All buffered blocks are guaranteed to be within
// workspace volume so just come to a controlled stop so position is not lost. When complete
// enter alarm mode.
if (sys.state == STATE_CYCLE) {
st_feed_hold();
while (sys.state == STATE_HOLD) {
protocol_execute_runtime();
if (sys.abort) { return; }
}
}
uint8_t idx;
for (idx=0; idx<N_AXIS; idx++) {
if (target[idx] > 0 || target[idx] < settings.max_travel[idx]) { // NOTE: max_travel is stored as negative
mc_reset(); // Issue system reset and ensure spindle and coolant are shutdown.
sys.execute |= EXEC_CRIT_EVENT; // Indicate soft limit critical event
protocol_execute_runtime(); // Execute to enter critical event loop and system abort
// Force feed hold if cycle is active. All buffered blocks are guaranteed to be within
// workspace volume so just come to a controlled stop so position is not lost. When complete
// enter alarm mode.
if (sys.state == STATE_CYCLE) {
st_feed_hold();
while (sys.state == STATE_HOLD) {
protocol_execute_runtime();
if (sys.abort) { return; }
}
}
mc_reset(); // Issue system reset and ensure spindle and coolant are shutdown.
sys.execute |= EXEC_CRIT_EVENT; // Indicate soft limit critical event
protocol_execute_runtime(); // Execute to enter critical event loop and system abort
return;
}
}
}

5
main.c
View File

@ -72,7 +72,8 @@ int main(void)
// Sync cleared gcode and planner positions to current system position, which is only
// cleared upon startup, not a reset/abort.
sys_sync_current_position();
plan_sync_position();
gc_sync_position();
// Reset system variables.
sys.abort = false;
@ -101,12 +102,12 @@ int main(void)
}
protocol_execute_runtime();
protocol_process(); // ... process the serial protocol
// When the serial protocol returns, there are no more characters in the serial read buffer to
// be processed and executed. This indicates that individual commands are being issued or
// streaming is finished. In either case, auto-cycle start, if enabled, any queued moves.
mc_auto_cycle_start();
protocol_process(); // ... process the serial protocol
}
return 0; /* never reached */

33
motion_control.c
View File

@ -72,7 +72,7 @@ void mc_line(float *target, float feed_rate, uint8_t invert_feed_rate)
else { break; }
} while (1);
plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], feed_rate, invert_feed_rate);
plan_buffer_line(target, feed_rate, invert_feed_rate);
// If idle, indicate to the system there is now a planned block in the buffer ready to cycle
// start. Otherwise ignore and continue on.
@ -148,8 +148,8 @@ void mc_arc(float *position, float *target, float *offset, uint8_t axis_0, uint8
This is important when there are successive arc motions.
*/
// Computes: cos_T = 1 - theta_per_segment^2/2, sin_T = theta_per_segment - theta_per_segment^3/6) in ~52usec
float cos_T = 2 - theta_per_segment*theta_per_segment;
float sin_T = theta_per_segment*0.16666667*(cos_T + 4);
float cos_T = 2.0 - theta_per_segment*theta_per_segment;
float sin_T = theta_per_segment*0.16666667*(cos_T + 4.0);
cos_T *= 0.5;
float arc_target[N_AXIS];
@ -229,37 +229,34 @@ void mc_go_home()
// At the same time, set up pull-off maneuver from axes limit switches that have been homed.
// This provides some initial clearance off the switches and should also help prevent them
// from falsely tripping when hard limits are enabled.
// TODO: Need to improve dir_mask[] to be more axes independent.
float pulloff_target[N_AXIS];
clear_vector_float(pulloff_target); // Zero pulloff target.
clear_vector_long(sys.position); // Zero current position for now.
uint8_t dir_mask[N_AXIS];
dir_mask[X_AXIS] = (1<<X_DIRECTION_BIT);
dir_mask[Y_AXIS] = (1<<Y_DIRECTION_BIT);
dir_mask[Z_AXIS] = (1<<Z_DIRECTION_BIT);
uint8_t i;
for (i=0; i<N_AXIS; i++) {
uint8_t idx;
for (idx=0; idx<N_AXIS; idx++) {
// Set up pull off targets and machine positions for limit switches homed in the negative
// direction, rather than the traditional positive. Leave non-homed positions as zero and
// do not move them.
if (HOMING_LOCATE_CYCLE & bit(i)) {
if (settings.homing_dir_mask & dir_mask[i]) {
pulloff_target[i] = settings.homing_pulloff-settings.max_travel[i];
sys.position[i] = -lround(settings.max_travel[i]*settings.steps_per_mm[i]);
// NOTE: settings.max_travel[] is stored as a negative value.
if (HOMING_LOCATE_CYCLE & bit(idx)) {
if ( settings.homing_dir_mask & get_direction_mask(idx) ) {
pulloff_target[idx] = settings.homing_pulloff+settings.max_travel[idx];
sys.position[idx] = lround(settings.max_travel[idx]*settings.steps_per_mm[idx]);
} else {
pulloff_target[i] = -settings.homing_pulloff;
pulloff_target[idx] = -settings.homing_pulloff;
}
}
}
sys_sync_current_position();
plan_sync_position(); // Sync planner position to home for pull-off move.
sys.state = STATE_IDLE; // Set system state to IDLE to complete motion and indicate homed.
mc_line(pulloff_target, settings.homing_seek_rate, false);
st_cycle_start(); // Move it. Nothing should be in the buffer except this motion.
plan_synchronize(); // Make sure the motion completes.
// The gcode parser position circumvented by the pull-off maneuver, so sync position vectors.
sys_sync_current_position();
// The gcode parser position circumvented by the pull-off maneuver, so sync position now.
gc_sync_position();
// If hard limits feature enabled, re-enable hard limits pin change register after homing cycle.
if (bit_istrue(settings.flags,BITFLAG_HARD_LIMIT_ENABLE)) { LIMIT_PCMSK |= LIMIT_MASK; }

15
nuts_bolts.c
View File

@ -140,9 +140,16 @@ void delay_us(uint32_t us)
}
}
// Syncs all internal position vectors to the current system position.
void sys_sync_current_position()
// Returns direction mask according to Grbl internal axis indexing.
uint8_t get_direction_mask(uint8_t axis_idx)
{
plan_set_current_position(sys.position[X_AXIS],sys.position[Y_AXIS],sys.position[Z_AXIS]);
gc_set_current_position(sys.position[X_AXIS],sys.position[Y_AXIS],sys.position[Z_AXIS]);
uint8_t axis_mask = 0;
switch( axis_idx ) {
case X_AXIS: axis_mask = (1<<X_DIRECTION_BIT); break;
case Y_AXIS: axis_mask = (1<<Y_DIRECTION_BIT); break;
case Z_AXIS: axis_mask = (1<<Z_DIRECTION_BIT); break;
}
return(axis_mask);
}

3
nuts_bolts.h
View File

@ -105,7 +105,6 @@ void delay_ms(uint16_t ms);
// Delays variable-defined microseconds. Compiler compatibility fix for _delay_us().
void delay_us(uint32_t us);
// Syncs Grbl's gcode and planner position variables with the system position.
void sys_sync_current_position();
uint8_t get_direction_mask(uint8_t i);
#endif

577
planner.c
View File

@ -2,8 +2,8 @@
planner.c - buffers movement commands and manages the acceleration profile plan
Part of Grbl
Copyright (c) 2009-2011 Simen Svale Skogsrud
Copyright (c) 2011-2013 Sungeun K. Jeon
Copyright (c) 2009-2011 Simen Svale Skogsrud
Copyright (c) 2011 Jens Geisler
Grbl is free software: you can redistribute it and/or modify
@ -34,11 +34,11 @@
#define SOME_LARGE_VALUE 1.0E+38 // Used by rapids and acceleration maximization calculations. Just needs
// to be larger than any feasible (mm/min)^2 or mm/sec^2 value.
static block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instructions
static volatile uint8_t block_buffer_head; // Index of the next block to be pushed
static plan_block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instructions
static volatile uint8_t block_buffer_tail; // Index of the block to process now
static uint8_t block_buffer_head; // Index of the next block to be pushed
static uint8_t next_buffer_head; // Index of the next buffer head
// static *block_t block_buffer_planned;
static uint8_t block_buffer_planned; // Index of the optimally planned block
// Define planner variables
typedef struct {
@ -47,7 +47,6 @@ typedef struct {
// i.e. arcs, canned cycles, and backlash compensation.
float previous_unit_vec[N_AXIS]; // Unit vector of previous path line segment
float previous_nominal_speed_sqr; // Nominal speed of previous path line segment
float last_x, last_y, last_z; // Target position of previous path line segment
} planner_t;
static planner_t pl;
@ -69,65 +68,7 @@ static uint8_t prev_block_index(uint8_t block_index)
block_index--;
return(block_index);
}
/* STEPPER VELOCITY PROFILE DEFINITION
less than nominal rate-> +
+--------+ <- nominal_rate /|\
/ \ / | \
initial_rate -> + \ / | + <- next->initial_rate
| + <- next->initial_rate / | |
+-------------+ initial_rate -> +----+--+
time --> ^ ^ ^ ^
| | | |
decelerate distance decelerate distance
Calculates trapezoid parameters for stepper algorithm. Each block velocity profiles can be
described as either a trapezoidal or a triangular shape. The trapezoid occurs when the block
reaches the nominal speed of the block and cruises for a period of time. A triangle occurs
when the nominal speed is not reached within the block. Some other special cases exist,
such as pure ac/de-celeration velocity profiles from beginning to end or a trapezoid that
has no deceleration period when the next block resumes acceleration.
The following function determines the type of velocity profile and stores the minimum required
information for the stepper algorithm to execute the calculated profiles. In this case, only
the new initial rate and n_steps until deceleration are computed, since the stepper algorithm
already handles acceleration and cruising and just needs to know when to start decelerating.
*/
static void calculate_trapezoid_for_block(block_t *block, float entry_speed_sqr, float exit_speed_sqr)
{
// Compute new initial rate for stepper algorithm
block->initial_rate = ceil(sqrt(entry_speed_sqr)*(RANADE_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
// TODO: Compute new nominal rate if a feedrate override occurs.
// block->nominal_rate = ceil(feed_rate*(RANADE_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
// Compute efficiency variable for following calculations. Removes a float divide and multiply.
// TODO: If memory allows, this can be kept in the block buffer since it doesn't change, even after feed holds.
float steps_per_mm_div_2_acc = block->step_event_count/(2*block->acceleration*block->millimeters);
// First determine intersection distance (in steps) from the exit point for a triangular profile.
// Computes: steps_intersect = steps/mm * ( distance/2 + (v_entry^2-v_exit^2)/(4*acceleration) )
int32_t intersect_distance = ceil( 0.5*(block->step_event_count + steps_per_mm_div_2_acc*(entry_speed_sqr-exit_speed_sqr)) );
// Check if this is a pure acceleration block by a intersection distance less than zero. Also
// prevents signed and unsigned integer conversion errors.
if (intersect_distance <= 0) {
block->decelerate_after = 0;
} else {
// Determine deceleration distance (in steps) from nominal speed to exit speed for a trapezoidal profile.
// Value is never negative. Nominal speed is always greater than or equal to the exit speed.
// Computes: steps_decelerate = steps/mm * ( (v_nominal^2 - v_exit^2)/(2*acceleration) )
block->decelerate_after = ceil(steps_per_mm_div_2_acc * (block->nominal_speed_sqr - exit_speed_sqr));
// The lesser of the two triangle and trapezoid distances always defines the velocity profile.
if (block->decelerate_after > intersect_distance) { block->decelerate_after = intersect_distance; }
// Finally, check if this is a pure deceleration block.
if (block->decelerate_after > block->step_event_count) { block->decelerate_after = block->step_event_count; }
}
}
/* PLANNER SPEED DEFINITION
+--------+ <- current->nominal_speed
@ -185,178 +126,130 @@ static void calculate_trapezoid_for_block(block_t *block, float entry_speed_sqr,
*/
static void planner_recalculate()
{
// float entry_speed_sqr;
// uint8_t block_index = block_buffer_head;
// block_t *previous = NULL;
// block_t *current = NULL;
// block_t *next;
// while (block_index != block_buffer_tail) {
// block_index = prev_block_index( block_index );
// next = current;
// current = previous;
// previous = &block_buffer[block_index];
//
// if (next && current) {
// if (next != block_buffer_planned) {
// if (previous == block_buffer_tail) { block_buffer_planned = next; }
// else {
//
// if (current->entry_speed_sqr != current->max_entry_speed_sqr) {
// current->recalculate_flag = true; // Almost always changes. So force recalculate.
// entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters;
// if (entry_speed_sqr < current->max_entry_speed_sqr) {
// current->entry_speed_sqr = entry_speed_sqr;
// } else {
// current->entry_speed_sqr = current->max_entry_speed_sqr;
// }
// } else {
// block_buffer_planned = current;
// }
// }
// } else {
// break;
// }
// }
// }
//
// block_index = block_buffer_planned;
// next = &block_buffer[block_index];
// current = prev_block_index(block_index);
// while (block_index != block_buffer_head) {
//
// // If the current block is an acceleration block, but it is not long enough to complete the
// // full speed change within the block, we need to adjust the exit speed accordingly. Entry
// // speeds have already been reset, maximized, and reverse planned by reverse planner.
// if (current->entry_speed_sqr < next->entry_speed_sqr) {
// // Compute block exit speed based on the current block speed and distance
// // Computes: v_exit^2 = v_entry^2 + 2*acceleration*distance
// entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters;
//
// // If it's less than the stored value, update the exit speed and set recalculate flag.
// if (entry_speed_sqr < next->entry_speed_sqr) {
// next->entry_speed_sqr = entry_speed_sqr;
// next->recalculate_flag = true;
// }
// }
//
// // Recalculate if current block entry or exit junction speed has changed.
// if (current->recalculate_flag || next->recalculate_flag) {
// // NOTE: Entry and exit factors always > 0 by all previous logic operations.
// calculate_trapezoid_for_block(current, current->entry_speed_sqr, next->entry_speed_sqr);
// current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed
// }
//
// current = next;
// next = &block_buffer[block_index];
// block_index = next_block_index( block_index );
// }
//
// // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated.
// calculate_trapezoid_for_block(next, next->entry_speed_sqr, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED);
// next->recalculate_flag = false;
// Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated.
uint8_t block_index = block_buffer_head;
plan_block_t *current = &block_buffer[block_index]; // Set as last/newest block in buffer
// TODO: No over-write protection exists for the executing block. For most cases this has proven to be ok, but
// for feed-rate overrides, something like this is essential. Place a request here to the stepper driver to
// find out where in the planner buffer is the a safe place to begin re-planning from.
// Ping the stepper algorithm to check if we can alter the parameters of the currently executing
// block. If not, skip it and work on the next block.
// TODO: Need to look into if there are conditions where this fails.
uint8_t block_buffer_safe = next_block_index( block_buffer_tail );
// TODO: Need to recompute buffer tail millimeters based on how much is completed.
if (block_buffer_safe == next_buffer_head) { // Only one safe block in buffer to operate on
// if (block_buffer_head != block_buffer_tail) {
float entry_speed_sqr;
block_buffer_planned = block_buffer_safe;
// calculate_trapezoid_for_block(current, 0.0, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED);
// Perform reverse planner pass. Skip the head(end) block since it is already initialized, and skip the
// tail(first) block to prevent over-writing of the initial entry speed.
uint8_t block_index = prev_block_index( block_buffer_head ); // Assume buffer is not empty.
block_t *current = &block_buffer[block_index]; // Head block-1 = Newly appended block
block_t *next;
if (block_index != block_buffer_tail) { block_index = prev_block_index( block_index ); }
while (block_index != block_buffer_tail) {
next = current;
current = &block_buffer[block_index];
// TODO: Determine maximum entry speed at junction for feedrate overrides, since they can alter
// the planner nominal speeds at any time. This calc could be done in the override handler, but
// this could require an additional variable to be stored to differentiate the programmed nominal
// speeds, max junction speed, and override speeds/scalar.
// If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
// If not, block in state of acceleration or deceleration. Reset entry speed to maximum and
// check for maximum allowable speed reductions to ensure maximum possible planned speed.
if (current->entry_speed_sqr != current->max_entry_speed_sqr) {
} else {
// TODO: need to account for the two block condition better. If the currently executing block
// is not safe, do we wait until its done? Can we treat the buffer head differently?
// Calculate trapezoid for the last/newest block.
current->entry_speed_sqr = min( current->max_entry_speed_sqr,
MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED + 2*current->acceleration*current->millimeters);
// calculate_trapezoid_for_block(current, current->entry_speed_sqr, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED);
current->entry_speed_sqr = current->max_entry_speed_sqr;
current->recalculate_flag = true; // Almost always changes. So force recalculate.
if (next->entry_speed_sqr < current->max_entry_speed_sqr) {
// Computes: v_entry^2 = v_exit^2 + 2*acceleration*distance
// Reverse Pass: Back plan the deceleration curve from the last block in buffer. Cease
// planning when: (1) the last optimal planned pointer is reached. (2) the safe block
// pointer is reached, whereby the planned pointer is updated.
float entry_speed_sqr;
plan_block_t *next;
block_index = prev_block_index(block_index);
while (block_index != block_buffer_planned) {
next = current;
current = &block_buffer[block_index];
// Exit loop and update planned pointer when the tail/safe block is reached.
if (block_index == block_buffer_safe) {
block_buffer_planned = block_buffer_safe;
break;
}
// Crudely maximize deceleration curve from the end of the non-optimally planned buffer to
// the optimal plan pointer. Forward pass will adjust and finish optimizing the plan.
if (current->entry_speed_sqr != current->max_entry_speed_sqr) {
entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters;
if (entry_speed_sqr < current->max_entry_speed_sqr) {
current->entry_speed_sqr = entry_speed_sqr;
} else {
current->entry_speed_sqr = current->max_entry_speed_sqr;
}
}
}
block_index = prev_block_index( block_index );
}
}
block_index = prev_block_index(block_index);
}
// Perform forward planner pass. Begins junction speed adjustments after tail(first) block.
// Also recalculate trapezoids, block by block, as the forward pass completes the plan.
block_index = next_block_index(block_buffer_tail);
next = &block_buffer[block_buffer_tail]; // Places tail(first) block into current
while (block_index != block_buffer_head) {
current = next;
next = &block_buffer[block_index];
// If the current block is an acceleration block, but it is not long enough to complete the
// full speed change within the block, we need to adjust the exit speed accordingly. Entry
// speeds have already been reset, maximized, and reverse planned by reverse planner.
// Forward Pass: Forward plan the acceleration curve from the planned pointer onward.
// Also scans for optimal plan breakpoints and appropriately updates the planned pointer.
block_index = block_buffer_planned; // Begin at buffer planned pointer
next = &block_buffer[prev_block_index(block_buffer_planned)]; // Set up for while loop
while (block_index != next_buffer_head) {
current = next;
next = &block_buffer[block_index];
// Any acceleration detected in the forward pass automatically moves the optimal planned
// pointer forward, since everything before this is all optimal. In other words, nothing
// can improve the plan from the buffer tail to the planned pointer by logic.
if (current->entry_speed_sqr < next->entry_speed_sqr) {
// Compute block exit speed based on the current block speed and distance
// Computes: v_exit^2 = v_entry^2 + 2*acceleration*distance
block_buffer_planned = block_index;
entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters;
// If it's less than the stored value, update the exit speed and set recalculate flag.
if (entry_speed_sqr < next->entry_speed_sqr) {
next->entry_speed_sqr = entry_speed_sqr;
next->recalculate_flag = true;
next->entry_speed_sqr = entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass set this.
}
}
// Recalculate if current block entry or exit junction speed has changed.
if (current->recalculate_flag || next->recalculate_flag) {
// NOTE: Entry and exit factors always > 0 by all previous logic operations.
calculate_trapezoid_for_block(current, current->entry_speed_sqr, next->entry_speed_sqr);
current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed
// Any block set at its maximum entry speed also creates an optimal plan up to this
// point in the buffer. The optimally planned pointer is updated.
if (next->entry_speed_sqr == next->max_entry_speed_sqr) {
block_buffer_planned = block_index;
}
block_index = next_block_index( block_index );
}
// Automatically recalculate trapezoid for all buffer blocks from last plan's optimal planned
// pointer to the end of the buffer, except the last block.
// calculate_trapezoid_for_block(current, current->entry_speed_sqr, next->entry_speed_sqr);
block_index = next_block_index( block_index );
}
}
// Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated.
calculate_trapezoid_for_block(next, next->entry_speed_sqr, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED);
next->recalculate_flag = false;
// }
}
void plan_init()
{
block_buffer_head = 0;
block_buffer_tail = block_buffer_head;
next_buffer_head = next_block_index(block_buffer_head);
// block_buffer_planned = block_buffer_head;
block_buffer_planned = block_buffer_head;
memset(&pl, 0, sizeof(pl)); // Clear planner struct
}
inline void plan_discard_current_block()
void plan_discard_current_block()
{
if (block_buffer_head != block_buffer_tail) {
block_buffer_tail = next_block_index( block_buffer_tail );
}
}
inline block_t *plan_get_current_block()
plan_block_t *plan_get_current_block()
{
if (block_buffer_head == block_buffer_tail) { return(NULL); }
return(&block_buffer[block_buffer_tail]);
}
plan_block_t *plan_get_block_by_index(uint8_t block_index)
{
if (block_buffer_head == block_index) { return(NULL); }
return(&block_buffer[block_index]);
}
// Returns the availability status of the block ring buffer. True, if full.
uint8_t plan_check_full_buffer()
{
@ -364,6 +257,7 @@ uint8_t plan_check_full_buffer()
return(false);
}
// Block until all buffered steps are executed or in a cycle state. Works with feed hold
// during a synchronize call, if it should happen. Also, waits for clean cycle end.
void plan_synchronize()
@ -374,43 +268,54 @@ void plan_synchronize()
}
}
// Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in
// millimeters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed
// Add a new linear movement to the buffer. target[N_AXIS] is the signed, absolute target position
// in millimeters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed
// rate is taken to mean "frequency" and would complete the operation in 1/feed_rate minutes.
// All position data passed to the planner must be in terms of machine position to keep the planner
// independent of any coordinate system changes and offsets, which are handled by the g-code parser.
// NOTE: Assumes buffer is available. Buffer checks are handled at a higher level by motion_control.
// Also the feed rate input value is used in three ways: as a normal feed rate if invert_feed_rate
// is false, as inverse time if invert_feed_rate is true, or as seek/rapids rate if the feed_rate
// value is negative (and invert_feed_rate always false).
void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert_feed_rate)
// In other words, the buffer head is never equal to the buffer tail. Also the feed rate input value
// is used in three ways: as a normal feed rate if invert_feed_rate is false, as inverse time if
// invert_feed_rate is true, or as seek/rapids rate if the feed_rate value is negative (and
// invert_feed_rate always false).
void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate)
{
// Prepare to set up new block
block_t *block = &block_buffer[block_buffer_head];
// Prepare and initialize new block
plan_block_t *block = &block_buffer[block_buffer_head];
block->step_event_count = 0;
block->millimeters = 0;
block->direction_bits = 0;
block->acceleration = SOME_LARGE_VALUE; // Scaled down to maximum acceleration later
// Calculate target position in absolute steps
int32_t target[N_AXIS];
target[X_AXIS] = lround(x*settings.steps_per_mm[X_AXIS]);
target[Y_AXIS] = lround(y*settings.steps_per_mm[Y_AXIS]);
target[Z_AXIS] = lround(z*settings.steps_per_mm[Z_AXIS]);
// Compute and store initial move distance data.
int32_t target_steps[N_AXIS];
float unit_vec[N_AXIS], delta_mm;
uint8_t idx;
for (idx=0; idx<N_AXIS; idx++) {
// Calculate target position in absolute steps. This conversion should be consistent throughout.
target_steps[idx] = lround(target[idx]*settings.steps_per_mm[idx]);
// Number of steps for each axis and determine max step events
block->steps[idx] = labs(target_steps[idx]-pl.position[idx]);
block->step_event_count = max(block->step_event_count, block->steps[idx]);
// Compute individual axes distance for move and prep unit vector calculations.
// NOTE: Computes true distance from converted step values.
delta_mm = (target_steps[idx] - pl.position[idx])/settings.steps_per_mm[idx];
unit_vec[idx] = delta_mm; // Store unit vector numerator. Denominator computed later.
// Set direction bits. Bit enabled always means direction is negative.
if (delta_mm < 0 ) { block->direction_bits |= get_direction_mask(idx); }
// Incrementally compute total move distance by Euclidean norm. First add square of each term.
block->millimeters += delta_mm*delta_mm;
}
block->millimeters = sqrt(block->millimeters); // Complete millimeters calculation with sqrt()
// Number of steps for each axis
block->steps_x = labs(target[X_AXIS]-pl.position[X_AXIS]);
block->steps_y = labs(target[Y_AXIS]-pl.position[Y_AXIS]);
block->steps_z = labs(target[Z_AXIS]-pl.position[Z_AXIS]);
block->step_event_count = max(block->steps_x, max(block->steps_y, block->steps_z));
// Bail if this is a zero-length block
if (block->step_event_count == 0) { return; };
if (block->step_event_count == 0) { return; }
// Compute path vector in terms of absolute step target and current positions
float delta_mm[N_AXIS];
delta_mm[X_AXIS] = x-pl.last_x;
delta_mm[Y_AXIS] = y-pl.last_y;
delta_mm[Z_AXIS] = z-pl.last_z;
block->millimeters = sqrt(delta_mm[X_AXIS]*delta_mm[X_AXIS] + delta_mm[Y_AXIS]*delta_mm[Y_AXIS] +
delta_mm[Z_AXIS]*delta_mm[Z_AXIS]);
// Adjust feed_rate value to mm/min depending on type of rate input (normal, inverse time, or rapids)
// TODO: Need to distinguish a rapids vs feed move for overrides. Some flag of some sort.
if (feed_rate < 0) { feed_rate = SOME_LARGE_VALUE; } // Scaled down to absolute max/rapids rate later
@ -422,117 +327,151 @@ void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert
// and axes properties as well.
// NOTE: This calculation assumes all axes are orthogonal (Cartesian) and works with ABC-axes,
// if they are also orthogonal/independent. Operates on the absolute value of the unit vector.
uint8_t i;
float unit_vec[N_AXIS], inverse_unit_vec_value;
float inverse_unit_vec_value;
float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple float divides
block->acceleration = SOME_LARGE_VALUE; // Scaled down to maximum acceleration in loop
for (i=0; i<N_AXIS; i++) {
if (delta_mm[i] == 0) {
unit_vec[i] = 0; // Store zero value. And avoid divide by zero.
} else {
// Compute unit vector and its absolute inverse value
unit_vec[i] = delta_mm[i]*inverse_millimeters;
inverse_unit_vec_value = abs(1.0/unit_vec[i]);
// Check and limit feed rate against max axis velocities and scale accelerations to maximums
feed_rate = min(feed_rate,settings.max_velocity[i]*inverse_unit_vec_value);
block->acceleration = min(block->acceleration,settings.acceleration[i]*inverse_unit_vec_value);
float junction_cos_theta = 0;
for (idx=0; idx<N_AXIS; idx++) {
if (unit_vec[idx] != 0) { // Avoid divide by zero.
unit_vec[idx] *= inverse_millimeters; // Complete unit vector calculation
inverse_unit_vec_value = abs(1.0/unit_vec[idx]); // Inverse to remove multiple float divides.
// Check and limit feed rate against max individual axis velocities and accelerations
feed_rate = min(feed_rate,settings.max_velocity[idx]*inverse_unit_vec_value);
block->acceleration = min(block->acceleration,settings.acceleration[idx]*inverse_unit_vec_value);
// Incrementally compute cosine of angle between previous and current path. Cos(theta) of the junction
// between the current move and the previous move is simply the dot product of the two unit vectors,
// where prev_unit_vec is negative. Used later to compute maximum junction speed.
junction_cos_theta -= pl.previous_unit_vec[idx] * unit_vec[idx];
}
}
// Compute nominal speed and rates
block->nominal_speed_sqr = feed_rate*feed_rate; // (mm/min)^2. Always > 0
block->nominal_rate = ceil(feed_rate*(RANADE_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
/* Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
Let a circle be tangent to both previous and current path line segments, where the junction
deviation is defined as the distance from the junction to the closest edge of the circle,
colinear with the circle center. The circular segment joining the two paths represents the
path of centripetal acceleration. Solve for max velocity based on max acceleration about the
radius of the circle, defined indirectly by junction deviation. This may be also viewed as
path width or max_jerk in the previous grbl version. This approach does not actually deviate
from path, but used as a robust way to compute cornering speeds, as it takes into account the
nonlinearities of both the junction angle and junction velocity.
NOTE: If the junction deviation value is finite, Grbl executes the motions in an exact path
mode (G61). If the junction deviation value is zero, Grbl will execute the motion in an exact
stop mode (G61.1) manner. In the future, if continuous mode (G64) is desired, the math here
is exactly the same. Instead of motioning all the way to junction point, the machine will
just follow the arc circle defined here. The Arduino doesn't have the CPU cycles to perform
a continuous mode path, but ARM-based microcontrollers most certainly do.
*/
// TODO: Acceleration need to be limited by the minimum of the two junctions.
// TODO: Need to setup a method to handle zero junction speeds when starting from rest.
if (block_buffer_head == block_buffer_tail) {
block->max_entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED;
} else {
// NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta).
float sin_theta_d2 = sqrt(0.5*(1.0-junction_cos_theta)); // Trig half angle identity. Always positive.
block->max_entry_speed_sqr = (block->acceleration * settings.junction_deviation * sin_theta_d2)/(1.0-sin_theta_d2);
}
// Compute the acceleration and distance traveled per step event for the stepper algorithm.
block->rate_delta = ceil(block->acceleration*
((RANADE_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic)
block->d_next = ceil((block->millimeters*RANADE_MULTIPLIER)/block->step_event_count); // (mult*mm/step)
// Compute direction bits. Bit enabled always means direction is negative.
block->direction_bits = 0;
if (unit_vec[X_AXIS] < 0) { block->direction_bits |= (1<<X_DIRECTION_BIT); }
if (unit_vec[Y_AXIS] < 0) { block->direction_bits |= (1<<Y_DIRECTION_BIT); }
if (unit_vec[Z_AXIS] < 0) { block->direction_bits |= (1<<Z_DIRECTION_BIT); }
// Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
// Let a circle be tangent to both previous and current path line segments, where the junction
// deviation is defined as the distance from the junction to the closest edge of the circle,
// colinear with the circle center. The circular segment joining the two paths represents the
// path of centripetal acceleration. Solve for max velocity based on max acceleration about the
// radius of the circle, defined indirectly by junction deviation. This may be also viewed as
// path width or max_jerk in the previous grbl version. This approach does not actually deviate
// from path, but used as a robust way to compute cornering speeds, as it takes into account the
// nonlinearities of both the junction angle and junction velocity.
// NOTE: If the junction deviation value is finite, Grbl executes the motions in an exact path
// mode (G61). If the junction deviation value is zero, Grbl will execute the motion in an exact
// stop mode (G61.1) manner. In the future, if continuous mode (G64) is desired, the math here
// is exactly the same. Instead of motioning all the way to junction point, the machine will
// just follow the arc circle defined here. The Arduino doesn't have the CPU cycles to perform
// a continuous mode path, but ARM-based microcontrollers most certainly do.
// Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
block->max_entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED;
if ((block_buffer_head != block_buffer_tail) && (pl.previous_nominal_speed_sqr > 0.0)) {
// Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
// NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
float cos_theta = - pl.previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
- pl.previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
- pl.previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
// Skip and use default max junction speed for 0 degree acute junction.
if (cos_theta < 0.95) {
block->max_entry_speed_sqr = min(block->nominal_speed_sqr,pl.previous_nominal_speed_sqr);
// Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
if (cos_theta > -0.95) {
// Compute maximum junction velocity based on maximum acceleration and junction deviation
float sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive.
block->max_entry_speed_sqr = min(block->max_entry_speed_sqr,
block->acceleration * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2));
}
}
}
// Initialize block entry speed. Compute block entry velocity backwards from user-defined MINIMUM_PLANNER_SPEED.
// TODO: This could be moved to the planner recalculate function.
block->entry_speed_sqr = min( block->max_entry_speed_sqr,
MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED + 2*block->acceleration*block->millimeters);
// Set new block to be recalculated for conversion to stepper data.
block->recalculate_flag = true;
// Store block nominal speed and rate
block->nominal_speed_sqr = feed_rate*feed_rate; // (mm/min). Always > 0
// block->nominal_rate = ceil(feed_rate*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
//
// // Compute and store acceleration and distance traveled per step event.
// block->rate_delta = ceil(block->acceleration*
// ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic)
// block->d_next = ceil((block->millimeters*INV_TIME_MULTIPLIER)/block->step_event_count); // (mult*mm/step)
// Update previous path unit_vector and nominal speed (squared)
memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // pl.previous_unit_vec[] = unit_vec[]
pl.previous_nominal_speed_sqr = block->nominal_speed_sqr;
// Update planner position
memcpy(pl.position, target, sizeof(target)); // pl.position[] = target[]
pl.last_x = x;
pl.last_y = y;
pl.last_z = z;
// Update buffer head and next buffer head indices
block_buffer_head = next_buffer_head;
next_buffer_head = next_block_index(block_buffer_head);
memcpy(pl.position, target_steps, sizeof(target_steps)); // pl.position[] = target_steps[]
planner_recalculate();
// Update buffer head and next buffer head indices.
// NOTE: The buffer head update is atomic since it's one byte. Performed after the new plan
// calculations to help prevent overwriting scenarios with adding a new block to a low buffer.
block_buffer_head = next_buffer_head;
next_buffer_head = next_block_index(block_buffer_head);
}
// Reset the planner position vectors. Called by the system abort/initialization routine.
void plan_set_current_position(int32_t x, int32_t y, int32_t z)
void plan_sync_position()
{
pl.position[X_AXIS] = x;
pl.position[Y_AXIS] = y;
pl.position[Z_AXIS] = z;
pl.last_x = x/settings.steps_per_mm[X_AXIS];
pl.last_y = y/settings.steps_per_mm[Y_AXIS];
pl.last_z = z/settings.steps_per_mm[Z_AXIS];
uint8_t idx;
for (idx=0; idx<N_AXIS; idx++) {
pl.position[idx] = sys.position[idx];
}
}
/* STEPPER VELOCITY PROFILE DEFINITION
less than nominal rate-> +
+--------+ <- nominal_rate /|\
/ \ / | \
initial_rate -> + \ / | + <- next->initial_rate
| + <- next->initial_rate / | |
+-------------+ initial_rate -> +----+--+
time --> ^ ^ ^ ^
| | | |
decelerate distance decelerate distance
Calculates trapezoid parameters for stepper algorithm. Each block velocity profiles can be
described as either a trapezoidal or a triangular shape. The trapezoid occurs when the block
reaches the nominal speed of the block and cruises for a period of time. A triangle occurs
when the nominal speed is not reached within the block. Some other special cases exist,
such as pure ac/de-celeration velocity profiles from beginning to end or a trapezoid that
has no deceleration period when the next block resumes acceleration.
The following function determines the type of velocity profile and stores the minimum required
information for the stepper algorithm to execute the calculated profiles. In this case, only
the new initial rate and n_steps until deceleration are computed, since the stepper algorithm
already handles acceleration and cruising and just needs to know when to start decelerating.
*/
int32_t calculate_trapezoid_for_block(uint8_t block_index)
{
plan_block_t *current_block = &block_buffer[block_index];
// Determine current block exit speed
float exit_speed_sqr;
uint8_t next_index = next_block_index(block_index);
plan_block_t *next_block = plan_get_block_by_index(next_index);
if (next_block == NULL) { exit_speed_sqr = 0; } // End of planner buffer. Zero speed.
else { exit_speed_sqr = next_block->entry_speed_sqr; } // Entry speed of next block
// First determine intersection distance (in steps) from the exit point for a triangular profile.
// Computes: steps_intersect = steps/mm * ( distance/2 + (v_entry^2-v_exit^2)/(4*acceleration) )
float intersect_distance = 0.5*( current_block->millimeters + (current_block->entry_speed_sqr-exit_speed_sqr)/(2*current_block->acceleration) );
// Check if this is a pure acceleration block by a intersection distance less than zero. Also
// prevents signed and unsigned integer conversion errors.
if (intersect_distance > 0 ) {
float decelerate_distance;
// Determine deceleration distance (in steps) from nominal speed to exit speed for a trapezoidal profile.
// Value is never negative. Nominal speed is always greater than or equal to the exit speed.
// Computes: steps_decelerate = steps/mm * ( (v_nominal^2 - v_exit^2)/(2*acceleration) )
decelerate_distance = (current_block->nominal_speed_sqr - exit_speed_sqr)/(2*current_block->acceleration);
// The lesser of the two triangle and trapezoid distances always defines the velocity profile.
if (decelerate_distance > intersect_distance) { decelerate_distance = intersect_distance; }
// Finally, check if this is a pure deceleration block.
if (decelerate_distance > current_block->millimeters) { decelerate_distance = current_block->millimeters; }
return(ceil(((current_block->millimeters-decelerate_distance)*current_block->step_event_count)/ current_block->millimeters));
}
return(0);
}
// Re-initialize buffer plan with a partially completed block, assumed to exist at the buffer tail.
// Called after a steppers have come to a complete stop for a feed hold and the cycle is stopped.
void plan_cycle_reinitialize(int32_t step_events_remaining)
{
block_t *block = &block_buffer[block_buffer_tail]; // Point to partially completed block
plan_block_t *block = &block_buffer[block_buffer_tail]; // Point to partially completed block
// Only remaining millimeters and step_event_count need to be updated for planner recalculate.
// Other variables (step_x, step_y, step_z, rate_delta, etc.) all need to remain the same to
@ -540,9 +479,9 @@ void plan_cycle_reinitialize(int32_t step_events_remaining)
block->millimeters = (block->millimeters*step_events_remaining)/block->step_event_count;
block->step_event_count = step_events_remaining;
// Re-plan from a complete stop. Reset planner entry speeds and flags.
// Re-plan from a complete stop. Reset planner entry speeds and buffer planned pointer.
block->entry_speed_sqr = 0.0;
block->max_entry_speed_sqr = 0.0;
block->recalculate_flag = true;
block->max_entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED;
block_buffer_planned = block_buffer_tail;
planner_recalculate();
}

49
planner.h
View File

@ -2,8 +2,8 @@
planner.h - buffers movement commands and manages the acceleration profile plan
Part of Grbl
Copyright (c) 2011-2013 Sungeun K. Jeon
Copyright (c) 2009-2011 Simen Svale Skogsrud
Copyright (c) 2011-2013 Sungeun K. Jeon
Grbl is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
@ -21,10 +21,11 @@
#ifndef planner_h
#define planner_h
#include "nuts_bolts.h"
// The number of linear motions that can be in the plan at any give time
#ifndef BLOCK_BUFFER_SIZE
#define BLOCK_BUFFER_SIZE 17
#define BLOCK_BUFFER_SIZE 18
#endif
// This struct is used when buffering the setup for each linear movement "nominal" values are as specified in
@ -32,43 +33,43 @@
typedef struct {
// Fields used by the bresenham algorithm for tracing the line
uint8_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h)
uint32_t steps_x, steps_y, steps_z; // Step count along each axis
int32_t step_event_count; // The number of step events required to complete this block
uint8_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h)
int32_t steps[N_AXIS]; // Step count along each axis
int32_t step_event_count; // The number of step events required to complete this block
// Fields used by the motion planner to manage acceleration
float nominal_speed_sqr; // The nominal speed for this block in mm/min
float entry_speed_sqr; // Entry speed at previous-current block junction in mm/min
float max_entry_speed_sqr; // Maximum allowable junction entry speed in mm/min
float millimeters; // The total travel of this block in mm
float acceleration;
uint8_t recalculate_flag; // Planner flag to recalculate trapezoids on entry junction
float nominal_speed_sqr; // Axis-limit adjusted nominal speed for this block in (mm/min)^2
float entry_speed_sqr; // Entry speed at previous-current block junction in (mm/min)^2
float max_entry_speed_sqr; // Maximum allowable junction entry speed in (mm/min)^2
float acceleration; // Axes-limit adjusted line acceleration in mm/min^2
float millimeters; // The total travel for this block to be executed in mm
// Settings for the trapezoid generator
uint32_t initial_rate; // The step rate at start of block
int32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive)
uint32_t decelerate_after; // The index of the step event on which to start decelerating
uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute
uint32_t d_next; // Scaled distance to next step
} block_t;
// int32_t decelerate_after; // The index of the step event on which to start decelerating
} plan_block_t;
// Initialize the motion plan subsystem
void plan_init();
// Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in
// millimaters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed
// Add a new linear movement to the buffer. target[N_AXIS] is the signed, absolute target position
// in millimeters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed
// rate is taken to mean "frequency" and would complete the operation in 1/feed_rate minutes.
void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert_feed_rate);
void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate);
// Called when the current block is no longer needed. Discards the block and makes the memory
// availible for new blocks.
void plan_discard_current_block();
// Gets the current block. Returns NULL if buffer empty
block_t *plan_get_current_block();
plan_block_t *plan_get_current_block();
plan_block_t *plan_get_block_by_index(uint8_t block_index);
int32_t calculate_trapezoid_for_block(uint8_t block_index);
// Reset the planner position vector (in steps)
void plan_set_current_position(int32_t x, int32_t y, int32_t z);
void plan_sync_position();
// Reinitialize plan with a partially completed block
void plan_cycle_reinitialize(int32_t step_events_remaining);

476
planner_old.c Normal file
View File

@ -0,0 +1,476 @@
/*
planner.c - buffers movement commands and manages the acceleration profile plan
Part of Grbl
Copyright (c) 2011-2013 Sungeun K. Jeon
Copyright (c) 2009-2011 Simen Svale Skogsrud
Copyright (c) 2011 Jens Geisler
Grbl is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
Grbl is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with Grbl. If not, see <http://www.gnu.org/licenses/>.
*/
/* The ring buffer implementation gleaned from the wiring_serial library by David A. Mellis. */
#include <inttypes.h>
#include <stdlib.h>
#include "planner.h"
#include "nuts_bolts.h"
#include "stepper.h"
#include "settings.h"
#include "config.h"
#include "protocol.h"
#define SOME_LARGE_VALUE 1.0E+38 // Used by rapids and acceleration maximization calculations. Just needs
// to be larger than any feasible (mm/min)^2 or mm/sec^2 value.
static block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instructions
static volatile uint8_t block_buffer_tail; // Index of the block to process now
static uint8_t block_buffer_head; // Index of the next block to be pushed
static uint8_t next_buffer_head; // Index of the next buffer head
static uint8_t block_buffer_planned; // Index of the optimally planned block
// Define planner variables
typedef struct {
int32_t position[N_AXIS]; // The planner position of the tool in absolute steps. Kept separate
// from g-code position for movements requiring multiple line motions,
// i.e. arcs, canned cycles, and backlash compensation.
float previous_unit_vec[N_AXIS]; // Unit vector of previous path line segment
float previous_nominal_speed_sqr; // Nominal speed of previous path line segment
float last_target[N_AXIS]; // Target position of previous path line segment
} planner_t;
static planner_t pl;
// Returns the index of the next block in the ring buffer
// NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication.
static uint8_t next_block_index(uint8_t block_index)
{
block_index++;
if (block_index == BLOCK_BUFFER_SIZE) { block_index = 0; }
return(block_index);
}
// Returns the index of the previous block in the ring buffer
static uint8_t prev_block_index(uint8_t block_index)
{
if (block_index == 0) { block_index = BLOCK_BUFFER_SIZE; }
block_index--;
return(block_index);
}
/* STEPPER VELOCITY PROFILE DEFINITION
less than nominal rate-> +
+--------+ <- nominal_rate /|\
/ \ / | \
initial_rate -> + \ / | + <- next->initial_rate
| + <- next->initial_rate / | |
+-------------+ initial_rate -> +----+--+
time --> ^ ^ ^ ^
| | | |
decelerate distance decelerate distance
Calculates trapezoid parameters for stepper algorithm. Each block velocity profiles can be
described as either a trapezoidal or a triangular shape. The trapezoid occurs when the block
reaches the nominal speed of the block and cruises for a period of time. A triangle occurs
when the nominal speed is not reached within the block. Some other special cases exist,
such as pure ac/de-celeration velocity profiles from beginning to end or a trapezoid that
has no deceleration period when the next block resumes acceleration.
The following function determines the type of velocity profile and stores the minimum required
information for the stepper algorithm to execute the calculated profiles. In this case, only
the new initial rate and n_steps until deceleration are computed, since the stepper algorithm
already handles acceleration and cruising and just needs to know when to start decelerating.
*/
static void calculate_trapezoid_for_block(block_t *block, float entry_speed_sqr, float exit_speed_sqr)
{
// Compute new initial rate for stepper algorithm
block->initial_rate = ceil(sqrt(entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
// TODO: Compute new nominal rate if a feedrate override occurs.
// block->nominal_rate = ceil(feed_rate*(RANADE_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
// Compute efficiency variable for following calculations. Removes a float divide and multiply.
// TODO: If memory allows, this can be kept in the block buffer since it doesn't change, even after feed holds.
float steps_per_mm_div_2_acc = block->step_event_count/(2*block->acceleration*block->millimeters);
// First determine intersection distance (in steps) from the exit point for a triangular profile.
// Computes: steps_intersect = steps/mm * ( distance/2 + (v_entry^2-v_exit^2)/(4*acceleration) )
int32_t intersect_distance = ceil( 0.5*(block->step_event_count + steps_per_mm_div_2_acc*(entry_speed_sqr-exit_speed_sqr)) );
// Check if this is a pure acceleration block by a intersection distance less than zero. Also
// prevents signed and unsigned integer conversion errors.
if (intersect_distance <= 0) {
block->decelerate_after = 0;
} else {
// Determine deceleration distance (in steps) from nominal speed to exit speed for a trapezoidal profile.
// Value is never negative. Nominal speed is always greater than or equal to the exit speed.
// Computes: steps_decelerate = steps/mm * ( (v_nominal^2 - v_exit^2)/(2*acceleration) )
block->decelerate_after = ceil(steps_per_mm_div_2_acc * (block->nominal_speed_sqr - exit_speed_sqr));
// The lesser of the two triangle and trapezoid distances always defines the velocity profile.
if (block->decelerate_after > intersect_distance) { block->decelerate_after = intersect_distance; }
// Finally, check if this is a pure deceleration block.
if (block->decelerate_after > block->step_event_count) { block->decelerate_after = block->step_event_count; }
}
}
/* PLANNER SPEED DEFINITION
+--------+ <- current->nominal_speed
/ \
current->entry_speed -> + \
| + <- next->entry_speed
+-------------+
time -->
Recalculates the motion plan according to the following algorithm:
1. Go over every block in reverse order and calculate a junction speed reduction (i.e. block_t.entry_speed)
so that:
a. The junction speed is equal to or less than the maximum junction speed limit
b. No speed reduction within one block requires faster deceleration than the acceleration limits.
c. The last (or newest appended) block is planned from a complete stop.
2. Go over every block in chronological (forward) order and dial down junction speed values if
a. The speed increase within one block would require faster acceleration than the acceleration limits.
When these stages are complete, all blocks have a junction entry speed that will allow all speed changes
to be performed using the overall limiting acceleration value, and where no junction speed is greater
than the max limit. In other words, it just computed the fastest possible velocity profile through all
buffered blocks, where the final buffered block is planned to come to a full stop when the buffer is fully
executed. Finally it will:
3. Convert the plan to data that the stepper algorithm needs. Only block trapezoids adjacent to a
a planner-modified junction speed with be updated, the others are assumed ok as is.
All planner computations(1)(2) are performed in floating point to minimize numerical round-off errors. Only
when planned values are converted to stepper rate parameters(3), these are integers. If another motion block
is added while executing, the planner will re-plan and update the stored optimal velocity profile as it goes.
Conceptually, the planner works like blowing up a balloon, where the balloon is the velocity profile. It's
constrained by the speeds at the beginning and end of the buffer, along with the maximum junction speeds and
nominal speeds of each block. Once a plan is computed, or balloon filled, this is the optimal velocity profile
through all of the motions in the buffer. Whenever a new block is added, this changes some of the limiting
conditions, or how the balloon is filled, so it has to be re-calculated to get the new optimal velocity profile.
Also, since the planner only computes on what's in the planner buffer, some motions with lots of short line
segments, like arcs, may seem to move slow. This is because there simply isn't enough combined distance traveled
in the entire buffer to accelerate up to the nominal speed and then decelerate to a stop at the end of the
buffer. There are a few simple solutions to this: (1) Maximize the machine acceleration. The planner will be
able to compute higher speed profiles within the same combined distance. (2) Increase line segment(s) distance.
The more combined distance the planner has to use, the faster it can go. (3) Increase the MINIMUM_PLANNER_SPEED.
Not recommended. This will change what speed the planner plans to at the end of the buffer. Can lead to lost
steps when coming to a stop. (4) [BEST] Increase the planner buffer size. The more combined distance, the
bigger the balloon, or faster it can go. But this is not possible for 328p Arduinos because its limited memory
is already maxed out. Future ARM versions should not have this issue, with look-ahead planner blocks numbering
up to a hundred or more.
NOTE: Since this function is constantly re-calculating for every new incoming block, it must be as efficient
as possible. For example, in situations like arc generation or complex curves, the short, rapid line segments
can execute faster than new blocks can be added, and the planner buffer will then starve and empty, leading
to weird hiccup-like jerky motions.
*/
static void planner_recalculate()
{
// Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated.
uint8_t block_index = block_buffer_head;
block_t *current = &block_buffer[block_index]; // Set as last/newest block in buffer
// Determine safe point for which to plan to.
uint8_t block_buffer_safe = next_block_index( block_buffer_tail );
if (block_buffer_safe == next_buffer_head) { // Only one safe block in buffer to operate on
block_buffer_planned = block_buffer_safe;
calculate_trapezoid_for_block(current, 0.0, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED);
} else {
// TODO: need to account for the two block condition better. If the currently executing block
// is not safe, do we wait until its done? Can we treat the buffer head differently?
// Calculate trapezoid for the last/newest block.
current->entry_speed_sqr = min( current->max_entry_speed_sqr,
MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED + 2*current->acceleration*current->millimeters);
calculate_trapezoid_for_block(current, current->entry_speed_sqr, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED);
// Reverse Pass: Back plan the deceleration curve from the last block in buffer. Cease
// planning when: (1) the last optimal planned pointer is reached. (2) the safe block
// pointer is reached, whereby the planned pointer is updated.
float entry_speed_sqr;
block_t *next;
block_index = prev_block_index(block_index);
while (block_index != block_buffer_planned) {
next = current;
current = &block_buffer[block_index];
// Exit loop and update planned pointer when the tail/safe block is reached.
if (block_index == block_buffer_safe) {
block_buffer_planned = block_buffer_safe;
break;
}
// Crudely maximize deceleration curve from the end of the non-optimally planned buffer to
// the optimal plan pointer. Forward pass will adjust and finish optimizing the plan.
if (current->entry_speed_sqr != current->max_entry_speed_sqr) {
entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters;
if (entry_speed_sqr < current->max_entry_speed_sqr) {
current->entry_speed_sqr = entry_speed_sqr;
} else {
current->entry_speed_sqr = current->max_entry_speed_sqr;
}
}
block_index = prev_block_index(block_index);
}
// Forward Pass: Forward plan the acceleration curve from the planned pointer onward.
// Also scans for optimal plan breakpoints and appropriately updates the planned pointer.
block_index = block_buffer_planned; // Begin at buffer planned pointer
next = &block_buffer[prev_block_index(block_buffer_planned)]; // Set up for while loop
while (block_index != next_buffer_head) {
current = next;
next = &block_buffer[block_index];
// Any acceleration detected in the forward pass automatically moves the optimal planned
// pointer forward, since everything before this is all optimal. In other words, nothing
// can improve the plan from the buffer tail to the planned pointer by logic.
if (current->entry_speed_sqr < next->entry_speed_sqr) {
block_buffer_planned = block_index;
entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters;
if (entry_speed_sqr < next->entry_speed_sqr) {
next->entry_speed_sqr = entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass set this.
}
}
// Any block set at its maximum entry speed also creates an optimal plan up to this
// point in the buffer. The optimally planned pointer is updated.
if (next->entry_speed_sqr == next->max_entry_speed_sqr) {
block_buffer_planned = block_index;
}
// Automatically recalculate trapezoid for all buffer blocks from last plan's optimal planned
// pointer to the end of the buffer, except the last block.
calculate_trapezoid_for_block(current, current->entry_speed_sqr, next->entry_speed_sqr);
block_index = next_block_index( block_index );
}
}
}
void plan_init()
{
block_buffer_tail = block_buffer_head;
next_buffer_head = next_block_index(block_buffer_head);
block_buffer_planned = block_buffer_head;
memset(&pl, 0, sizeof(pl)); // Clear planner struct
}
inline void plan_discard_current_block()
{
if (block_buffer_head != block_buffer_tail) {
block_buffer_tail = next_block_index( block_buffer_tail );
}
}
inline block_t *plan_get_current_block()
{
if (block_buffer_head == block_buffer_tail) { return(NULL); }
return(&block_buffer[block_buffer_tail]);
}
// Returns the availability status of the block ring buffer. True, if full.
uint8_t plan_check_full_buffer()
{
if (block_buffer_tail == next_buffer_head) { return(true); }
return(false);
}
// Block until all buffered steps are executed or in a cycle state. Works with feed hold
// during a synchronize call, if it should happen. Also, waits for clean cycle end.
void plan_synchronize()
{
while (plan_get_current_block() || sys.state == STATE_CYCLE) {
protocol_execute_runtime(); // Check and execute run-time commands
if (sys.abort) { return; } // Check for system abort
}
}
// Add a new linear movement to the buffer. target[N_AXIS] is the signed, absolute target position
// in millimeters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed
// rate is taken to mean "frequency" and would complete the operation in 1/feed_rate minutes.
// All position data passed to the planner must be in terms of machine position to keep the planner
// independent of any coordinate system changes and offsets, which are handled by the g-code parser.
// NOTE: Assumes buffer is available. Buffer checks are handled at a higher level by motion_control.
// In other words, the buffer head is never equal to the buffer tail. Also the feed rate input value
// is used in three ways: as a normal feed rate if invert_feed_rate is false, as inverse time if
// invert_feed_rate is true, or as seek/rapids rate if the feed_rate value is negative (and
// invert_feed_rate always false).
void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate)
{
// Prepare and initialize new block
block_t *block = &block_buffer[block_buffer_head];
block->step_event_count = 0;
block->millimeters = 0;
block->direction_bits = 0;
block->acceleration = SOME_LARGE_VALUE; // Scaled down to maximum acceleration later
// Compute and store initial move distance data.
int32_t target_steps[N_AXIS];
float unit_vec[N_AXIS], delta_mm;
uint8_t idx;
for (idx=0; idx<N_AXIS; idx++) {
// Calculate target position in absolute steps
target_steps[idx] = lround(target[idx]*settings.steps_per_mm[idx]);
// Number of steps for each axis and determine max step events
block->steps[idx] = labs(target_steps[idx]-pl.position[idx]);
block->step_event_count = max(block->step_event_count, block->steps[idx]);
// Compute individual axes distance for move and prep unit vector calculations.
delta_mm = target[idx] - pl.last_target[idx];
unit_vec[idx] = delta_mm; // Store unit vector numerator. Denominator computed later.
// Incrementally compute total move distance by Euclidean norm
block->millimeters += delta_mm*delta_mm;
// Set direction bits. Bit enabled always means direction is negative.
if (delta_mm < 0 ) { block->direction_bits |= get_direction_mask(idx); }
}
block->millimeters = sqrt(block->millimeters); // Complete millimeters calculation
// Bail if this is a zero-length block
if (block->step_event_count == 0) { return; }
// Adjust feed_rate value to mm/min depending on type of rate input (normal, inverse time, or rapids)
// TODO: Need to distinguish a rapids vs feed move for overrides. Some flag of some sort.
if (feed_rate < 0) { feed_rate = SOME_LARGE_VALUE; } // Scaled down to absolute max/rapids rate later
else if (invert_feed_rate) { feed_rate = block->millimeters/feed_rate; }
// Calculate the unit vector of the line move and the block maximum feed rate and acceleration limited
// by the maximum possible values. Block rapids rates are computed or feed rates are scaled down so
// they don't exceed the maximum axes velocities. The block acceleration is maximized based on direction
// and axes properties as well.
// NOTE: This calculation assumes all axes are orthogonal (Cartesian) and works with ABC-axes,
// if they are also orthogonal/independent. Operates on the absolute value of the unit vector.
float inverse_unit_vec_value;
float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple float divides
float junction_cos_theta = 0;
for (idx=0; idx<N_AXIS; idx++) {
if (unit_vec[idx] != 0) { // Avoid divide by zero.
unit_vec[idx] *= inverse_millimeters; // Complete unit vector calculation
inverse_unit_vec_value = abs(1.0/unit_vec[idx]); // Inverse to remove multiple float divides.
// Check and limit feed rate against max individual axis velocities and accelerations
feed_rate = min(feed_rate,settings.max_velocity[idx]*inverse_unit_vec_value);
block->acceleration = min(block->acceleration,settings.acceleration[idx]*inverse_unit_vec_value);
// Incrementally compute cosine of angle between previous and current path. Cos(theta) of the junction
// between the current move and the previous move is simply the dot product of the two unit vectors,
// where prev_unit_vec is negative. Used later to compute maximum junction speed.
junction_cos_theta -= pl.previous_unit_vec[idx] * unit_vec[idx];
}
}
/* Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
Let a circle be tangent to both previous and current path line segments, where the junction
deviation is defined as the distance from the junction to the closest edge of the circle,
colinear with the circle center. The circular segment joining the two paths represents the
path of centripetal acceleration. Solve for max velocity based on max acceleration about the
radius of the circle, defined indirectly by junction deviation. This may be also viewed as
path width or max_jerk in the previous grbl version. This approach does not actually deviate
from path, but used as a robust way to compute cornering speeds, as it takes into account the
nonlinearities of both the junction angle and junction velocity.
NOTE: If the junction deviation value is finite, Grbl executes the motions in an exact path
mode (G61). If the junction deviation value is zero, Grbl will execute the motion in an exact
stop mode (G61.1) manner. In the future, if continuous mode (G64) is desired, the math here
is exactly the same. Instead of motioning all the way to junction point, the machine will
just follow the arc circle defined here. The Arduino doesn't have the CPU cycles to perform
a continuous mode path, but ARM-based microcontrollers most certainly do.
*/
// TODO: Acceleration need to be limited by the minimum of the two junctions.
// TODO: Need to setup a method to handle zero junction speeds when starting from rest.
if (block_buffer_head == block_buffer_tail) {
block->max_entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED;
} else {
// NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta).
float sin_theta_d2 = sqrt(0.5*(1.0-junction_cos_theta)); // Trig half angle identity. Always positive.
block->max_entry_speed_sqr = (block->acceleration * settings.junction_deviation * sin_theta_d2)/(1.0-sin_theta_d2);
}
// Store block nominal speed and rate
block->nominal_speed_sqr = feed_rate*feed_rate; // (mm/min)^2. Always > 0
block->nominal_rate = ceil(feed_rate*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
// Compute and store acceleration and distance traveled per step event.
block->rate_delta = ceil(block->acceleration*
((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic)
block->d_next = ceil((block->millimeters*INV_TIME_MULTIPLIER)/block->step_event_count); // (mult*mm/step)
// Update previous path unit_vector and nominal speed (squared)
memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // pl.previous_unit_vec[] = unit_vec[]
pl.previous_nominal_speed_sqr = block->nominal_speed_sqr;
// Update planner position
memcpy(pl.position, target_steps, sizeof(target_steps)); // pl.position[] = target_steps[]
memcpy(pl.last_target, target, sizeof(target)); // pl.last_target[] = target[]
planner_recalculate();
// Update buffer head and next buffer head indices.
// NOTE: The buffer head update is atomic since it's one byte. Performed after the new plan
// calculations to help prevent overwriting scenarios with adding a new block to a low buffer.
block_buffer_head = next_buffer_head;
next_buffer_head = next_block_index(block_buffer_head);
}
// Reset the planner position vectors. Called by the system abort/initialization routine.
void plan_sync_position()
{
uint8_t idx;
for (idx=0; idx<N_AXIS; idx++) {
pl.position[idx] = sys.position[idx];
pl.last_target[idx] = sys.position[idx]/settings.steps_per_mm[idx];
}
}
// Re-initialize buffer plan with a partially completed block, assumed to exist at the buffer tail.
// Called after a steppers have come to a complete stop for a feed hold and the cycle is stopped.
void plan_cycle_reinitialize(int32_t step_events_remaining)
{
block_t *block = &block_buffer[block_buffer_tail]; // Point to partially completed block
// Only remaining millimeters and step_event_count need to be updated for planner recalculate.
// Other variables (step_x, step_y, step_z, rate_delta, etc.) all need to remain the same to
// ensure the original planned motion is resumed exactly.
block->millimeters = (block->millimeters*step_events_remaining)/block->step_event_count;
block->step_event_count = step_events_remaining;
// Re-plan from a complete stop. Reset planner entry speeds and buffer planned pointer.
block->entry_speed_sqr = 0.0;
block->max_entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED;
block_buffer_planned = block_buffer_tail;
planner_recalculate();
}

83
planner_old.h Normal file
View File

@ -0,0 +1,83 @@
/*
planner.h - buffers movement commands and manages the acceleration profile plan
Part of Grbl
Copyright (c) 2011-2013 Sungeun K. Jeon
Copyright (c) 2009-2011 Simen Svale Skogsrud
Grbl is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
Grbl is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with Grbl. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef planner_h
#define planner_h
#include "nuts_bolts.h"
// The number of linear motions that can be in the plan at any give time
#ifndef BLOCK_BUFFER_SIZE
#define BLOCK_BUFFER_SIZE 17
#endif
// This struct is used when buffering the setup for each linear movement "nominal" values are as specified in
// the source g-code and may never actually be reached if acceleration management is active.
typedef struct {
// Fields used by the bresenham algorithm for tracing the line
uint8_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h)
uint32_t steps[N_AXIS]; // Step count along each axis
int32_t step_event_count; // The number of step events required to complete this block
// Fields used by the motion planner to manage acceleration
float nominal_speed_sqr; // Axis-limit adjusted nominal speed for this block in (mm/min)^2
float entry_speed_sqr; // Entry speed at previous-current block junction in (mm/min)^2
float max_entry_speed_sqr; // Maximum allowable junction entry speed in (mm/min)^2
float millimeters; // The total travel of this block in mm
float acceleration; // Axes-limit adjusted line acceleration in mm/min^2
// Settings for the trapezoid generator
uint32_t initial_rate; // The step rate at start of block
int32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive)
uint32_t decelerate_after; // The index of the step event on which to start decelerating
uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute
uint32_t d_next; // Scaled distance to next step
} block_t;
// Initialize the motion plan subsystem
void plan_init();
// Add a new linear movement to the buffer. target[N_AXIS] is the signed, absolute target position
// in millimeters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed
// rate is taken to mean "frequency" and would complete the operation in 1/feed_rate minutes.
void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate);
// Called when the current block is no longer needed. Discards the block and makes the memory
// availible for new blocks.
void plan_discard_current_block();
// Gets the current block. Returns NULL if buffer empty
block_t *plan_get_current_block();
// Reset the planner position vector (in steps)
void plan_sync_position();
// Reinitialize plan with a partially completed block
void plan_cycle_reinitialize(int32_t step_events_remaining);
// Returns the status of the block ring buffer. True, if buffer is full.
uint8_t plan_check_full_buffer();
// Block until all buffered steps are executed
void plan_synchronize();
#endif

2
print.c
View File

@ -77,7 +77,7 @@ void print_uint8_base2(uint8_t n)
serial_write('0' + buf[i - 1]);
}
static void print_uint32_base10(unsigned long n)
void print_uint32_base10(unsigned long n)
{
unsigned char buf[10];
uint8_t i = 0;

2
print.h
View File

@ -31,6 +31,8 @@ void printPgmString(const char *s);
void printInteger(long n);
void print_uint32_base10(uint32_t n);
void print_uint8_base2(uint8_t n);
void printFloat(float n);

1
protocol.c
View File

@ -104,6 +104,7 @@ ISR(PINOUT_INT_vect)
// limit switches, or the main program.
void protocol_execute_runtime()
{
st_prep_buffer();
if (sys.execute) { // Enter only if any bit flag is true
uint8_t rt_exec = sys.execute; // Avoid calling volatile multiple times

6
report.c
View File

@ -157,9 +157,9 @@ void report_grbl_settings() {
printPgmString(PSTR(" (z v_max, mm/min)\r\n$6=")); printFloat(settings.acceleration[X_AXIS]/(60*60)); // Convert from mm/min^2 for human readability
printPgmString(PSTR(" (x accel, mm/sec^2)\r\n$7=")); printFloat(settings.acceleration[Y_AXIS]/(60*60)); // Convert from mm/min^2 for human readability
printPgmString(PSTR(" (y accel, mm/sec^2)\r\n$8=")); printFloat(settings.acceleration[Z_AXIS]/(60*60)); // Convert from mm/min^2 for human readability
printPgmString(PSTR(" (z accel, mm/sec^2)\r\n$9=")); printFloat(settings.max_travel[X_AXIS]);
printPgmString(PSTR(" (x max travel, mm)\r\n$10=")); printFloat(settings.max_travel[Y_AXIS]);
printPgmString(PSTR(" (y max travel, mm)\r\n$11=")); printFloat(settings.max_travel[Z_AXIS]);
printPgmString(PSTR(" (z accel, mm/sec^2)\r\n$9=")); printFloat(-settings.max_travel[X_AXIS]); // Grbl internally store this as negative.
printPgmString(PSTR(" (x max travel, mm)\r\n$10=")); printFloat(-settings.max_travel[Y_AXIS]); // Grbl internally store this as negative.
printPgmString(PSTR(" (y max travel, mm)\r\n$11=")); printFloat(-settings.max_travel[Z_AXIS]); // Grbl internally store this as negative.
printPgmString(PSTR(" (z max travel, mm)\r\n$12=")); printInteger(settings.pulse_microseconds);
printPgmString(PSTR(" (step pulse, usec)\r\n$13=")); printFloat(settings.default_feed_rate);
printPgmString(PSTR(" (default feed, mm/min)\r\n$14=")); printInteger(settings.invert_mask);

11
settings.c
View File

@ -169,9 +169,9 @@ uint8_t settings_store_global_setting(int parameter, float value) {
case 6: settings.acceleration[X_AXIS] = value*60*60; break; // Convert to mm/min^2 for grbl internal use.
case 7: settings.acceleration[Y_AXIS] = value*60*60; break; // Convert to mm/min^2 for grbl internal use.
case 8: settings.acceleration[Z_AXIS] = value*60*60; break; // Convert to mm/min^2 for grbl internal use.
case 9: settings.max_travel[X_AXIS] = value; break;
case 10: settings.max_travel[Y_AXIS] = value; break;
case 11: settings.max_travel[Z_AXIS] = value; break;
case 9: settings.max_travel[X_AXIS] = -value; break; // Store as negative for grbl internal use.
case 10: settings.max_travel[Y_AXIS] = -value; break; // Store as negative for grbl internal use.
case 11: settings.max_travel[Z_AXIS] = -value; break; // Store as negative for grbl internal use.
case 12:
if (value < 3) { return(STATUS_SETTING_STEP_PULSE_MIN); }
settings.pulse_microseconds = round(value); break;
@ -206,7 +206,10 @@ uint8_t settings_store_global_setting(int parameter, float value) {
break;
case 24:
if (value) { settings.flags |= BITFLAG_HOMING_ENABLE; }
else { settings.flags &= ~BITFLAG_HOMING_ENABLE; }
else {
settings.flags &= ~BITFLAG_HOMING_ENABLE;
settings.flags &= ~BITFLAG_SOFT_LIMIT_ENABLE;
}
break;
case 25: settings.homing_dir_mask = trunc(value); break;
case 26: settings.homing_feed_rate = value; break;

571
stepper.c
View File

@ -19,20 +19,30 @@
along with Grbl. If not, see <http://www.gnu.org/licenses/>.
*/
/* The timer calculations of this module informed by the 'RepRap cartesian firmware' by Zack Smith
and Philipp Tiefenbacher. */
#include <avr/interrupt.h>
#include "stepper.h"
#include "config.h"
#include "settings.h"
#include "planner.h"
#include "nuts_bolts.h"
// Some useful constants
#define TICKS_PER_MICROSECOND (F_CPU/1000000)
#define CRUISE_RAMP 0
#define ACCEL_RAMP 1
#define DECEL_RAMP 2
#define RAMP_NOOP_CRUISE 0
#define RAMP_ACCEL 1
#define RAMP_DECEL 2
#define LOAD_NOOP 0
#define LOAD_LINE 1
#define LOAD_BLOCK 2
#define ST_NOOP 0
#define ST_END_OF_BLOCK 1
#define ST_DECEL 2
#define ST_DECEL_EOB 3
#define SEGMENT_BUFFER_SIZE 10
// Stepper state variable. Contains running data and trapezoid variables.
typedef struct {
@ -40,44 +50,92 @@ typedef struct {
int32_t counter_x, // Counter variables for the bresenham line tracer
counter_y,
counter_z;
uint32_t event_count; // Total event count. Retained for feed holds.
uint32_t step_events_remaining; // Steps remaining in motion
uint8_t segment_steps_remaining; // Steps remaining in line motion
// Used by Pramod Ranade inverse time algorithm
int32_t delta_d; // Ranade distance traveled per interrupt tick
int32_t d_counter; // Ranade distance traveled since last step event
uint8_t ramp_count; // Acceleration interrupt tick counter.
uint8_t ramp_type; // Ramp type variable.
uint8_t execute_step; // Flags step execution for each interrupt.
// Used by inverse time algorithm to track step rate
int32_t counter_d; // Inverse time distance traveled since last step event
uint32_t delta_d; // Inverse time distance traveled per interrupt tick
uint32_t d_per_tick;
// Used by the stepper driver interrupt
uint8_t execute_step; // Flags step execution for each interrupt.
uint8_t step_pulse_time; // Step pulse reset time after step rise
uint8_t out_bits; // The next stepping-bits to be output
uint8_t load_flag;
uint8_t ramp_count;
uint8_t ramp_type;
} stepper_t;
static stepper_t st;
static block_t *current_block; // A pointer to the block currently being traced
// Used by the stepper driver interrupt
static uint8_t step_pulse_time; // Step pulse reset time after step rise
static uint8_t out_bits; // The next stepping-bits to be output
// Stores stepper buffer common data. Can change planner mid-block in special conditions.
typedef struct {
int32_t step_events_remaining; // Tracks step event count for the executing planner block
uint32_t d_next; // Scaled distance to next step
uint32_t initial_rate; // Initialized step rate at re/start of a planner block
uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute
uint32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive)
int32_t decelerate_after;
float mm_per_step;
} st_data_t;
static st_data_t segment_data[SEGMENT_BUFFER_SIZE];
// NOTE: If the main interrupt is guaranteed to be complete before the next interrupt, then
// this blocking variable is no longer needed. Only here for safety reasons.
static volatile uint8_t busy; // True when "Stepper Driver Interrupt" is being serviced. Used to avoid retriggering that handler.
// Primary stepper motion buffer
typedef struct {
uint8_t n_step;
uint8_t st_data_index;
uint8_t flag;
} st_segment_t;
static st_segment_t segment_buffer[SEGMENT_BUFFER_SIZE];
// __________________________
// /| |\ _________________ ^
// / | | \ /| |\ |
// / | | \ / | | \ s
// / | | | | | \ p
// / | | | | | \ e
// +-----+------------------------+---+--+---------------+----+ e
// | BLOCK 1 | BLOCK 2 | d
//
// time ----->
//
// The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta
// until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after
// after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as
// +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND.
static volatile uint8_t segment_buffer_tail;
static uint8_t segment_buffer_head;
static uint8_t segment_next_head;
static volatile uint8_t busy; // Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though.
static plan_block_t *pl_current_block; // A pointer to the planner block currently being traced
static st_segment_t *st_current_segment;
static st_data_t *st_current_data;
static plan_block_t *pl_prep_block; // A pointer to the planner block being prepped into the stepper buffer
static uint8_t pl_prep_index;
static st_data_t *st_prep_data;
static uint8_t st_data_prep_index;
// Returns the index of the next block in the ring buffer
// NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication.
static uint8_t next_block_index(uint8_t block_index)
{
block_index++;
if (block_index == SEGMENT_BUFFER_SIZE) { block_index = 0; }
return(block_index);
}
static uint8_t next_block_pl_index(uint8_t block_index)
{
block_index++;
if (block_index == 18) { block_index = 0; }
return(block_index);
}
/* __________________________
/| |\ _________________ ^
/ | | \ /| |\ |
/ | | \ / | | \ s
/ | | | | | \ p
/ | | | | | \ e
+-----+------------------------+---+--+---------------+----+ e
| BLOCK 1 | BLOCK 2 | d
time ----->
The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta
until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after
after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as
+/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND.
*/
// Stepper state initialization. Cycle should only start if the st.cycle_start flag is
// enabled. Startup init and limits call this function but shouldn't start the cycle.
@ -91,14 +149,17 @@ void st_wake_up()
}
if (sys.state == STATE_CYCLE) {
// Initialize stepper output bits
out_bits = settings.invert_mask;
st.out_bits = settings.invert_mask;
// Initialize step pulse timing from settings.
step_pulse_time = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3);
st.step_pulse_time = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3);
// Enable stepper driver interrupt
st.execute_step = false;
st.load_flag = LOAD_BLOCK;
TCNT2 = 0; // Clear Timer2
TIMSK2 |= (1<<OCIE2A); // Enable Timer2 Compare Match A interrupt
TCCR2B = (1<<CS21); // Begin Timer2. Full speed, 1/8 prescaler
}
}
@ -125,20 +186,20 @@ void st_go_idle()
}
// "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Grbl. It is based
// on the Pramod Ranade inverse time stepper algorithm, where a timer ticks at a constant
// frequency and uses time-distance counters to track when its the approximate time for any
// step event. However, the Ranade algorithm, as described, is susceptible to numerical round-off,
// meaning that some axes steps may not execute for a given multi-axis motion.
// Grbl's algorithm slightly differs by using a single Ranade time-distance counter to manage
// a Bresenham line algorithm for multi-axis step events which ensures the number of steps for
// each axis are executed exactly. In other words, it uses a Bresenham within a Bresenham algorithm,
// where one tracks time(Ranade) and the other steps.
// This interrupt pops blocks from the block_buffer and executes them by pulsing the stepper pins
// appropriately. It is supported by The Stepper Port Reset Interrupt which it uses to reset the
// stepper port after each pulse. The bresenham line tracer algorithm controls all three stepper
// outputs simultaneously with these two interrupts.
//
/* "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Grbl. It is based
on the Pramod Ranade inverse time stepper algorithm, where a timer ticks at a constant
frequency and uses time-distance counters to track when its the approximate time for any
step event. However, the Ranade algorithm, as described, is susceptible to numerical round-off,
meaning that some axes steps may not execute for a given multi-axis motion.
Grbl's algorithm slightly differs by using a single Ranade time-distance counter to manage
a Bresenham line algorithm for multi-axis step events which ensures the number of steps for
each axis are executed exactly. In other words, it uses a Bresenham within a Bresenham algorithm,
where one tracks time(Ranade) and the other steps.
This interrupt pops blocks from the block_buffer and executes them by pulsing the stepper pins
appropriately. It is supported by The Stepper Port Reset Interrupt which it uses to reset the
stepper port after each pulse. The bresenham line tracer algorithm controls all three stepper
outputs simultaneously with these two interrupts.
*/
// NOTE: Average time in this ISR is: 5 usec iterating timers only, 20-25 usec with step event, or
// 15 usec when popping a block. So, ensure Ranade frequency and step pulse times work with this.
ISR(TIMER2_COMPA_vect)
@ -150,150 +211,171 @@ ISR(TIMER2_COMPA_vect)
// before any step pulse due to algorithm design.
if (st.execute_step) {
st.execute_step = false;
STEPPING_PORT = ( STEPPING_PORT & ~(DIRECTION_MASK | STEP_MASK) ) | out_bits;
TCNT0 = step_pulse_time; // Reload Timer0 counter.
STEPPING_PORT = ( STEPPING_PORT & ~(DIRECTION_MASK | STEP_MASK) ) | st.out_bits;
TCNT0 = st.step_pulse_time; // Reload Timer0 counter.
TCCR0B = (1<<CS21); // Begin Timer0. Full speed, 1/8 prescaler
}
busy = true;
sei(); // Re-enable interrupts. This ISR will still finish before returning to main program.
// If there is no current block, attempt to pop one from the buffer
if (current_block == NULL) {
// Anything in the buffer? If so, initialize next motion.
current_block = plan_get_current_block();
if (current_block != NULL) {
// By algorithm design, the loading of the next block never coincides with a step event,
// since there is always one Ranade timer tick before a step event occurs. This means
// that the Bresenham counter math never is performed at the same time as the loading
// of a block, hence helping minimize total time spent in this interrupt.
sei(); // Re-enable interrupts to allow Stepper Port Reset Interrupt to fire on-time.
// NOTE: The remaining code in this ISR will finish before returning to main program.
// If there is no step segment, attempt to pop one from the stepper buffer
if (st.load_flag != LOAD_NOOP) {
// Anything in the buffer? If so, load and initialize next step segment.
if (segment_buffer_head != segment_buffer_tail) {
// Initialize direction bits for block
out_bits = current_block->direction_bits ^ settings.invert_mask;
st.execute_step = true; // Set flag to set direction bits.
// NOTE: Loads after a step event. At high rates above 1/2 ISR frequency, there is
// a small chance that this will load at the same time as a step event. Hopefully,
// the overhead for this loading event isn't too much.. possibly 2-5 usec.
// NOTE: The stepper algorithm must control the planner buffer tail as it completes
// the block moves. Otherwise, a feed hold can leave a few step buffer line moves
// without the correct planner block information.
// Initialize Bresenham variables
st.counter_x = (current_block->step_event_count >> 1);
st.counter_y = st.counter_x;
st.counter_z = st.counter_x;
st.event_count = current_block->step_event_count;
st.step_events_remaining = st.event_count;
st_current_segment = &segment_buffer[segment_buffer_tail];
// During feed hold, do not update Ranade counter, rate, or ramp type. Keep decelerating.
if (sys.state == STATE_CYCLE) {
// Initialize Ranade variables
st.d_counter = current_block->d_next;
st.delta_d = current_block->initial_rate;
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2;
// Load number of steps to execute from stepper buffer
st.segment_steps_remaining = st_current_segment->n_step;
// Check if the counters need to be reset for a new planner block
if (st.load_flag == LOAD_BLOCK) {
pl_current_block = plan_get_current_block(); // Should always be there. Stepper buffer handles this.
st_current_data = &segment_data[segment_buffer[segment_buffer_tail].st_data_index]; //st_current_segment->st_data_index];
// Initialize ramp type.
if (st.step_events_remaining == current_block->decelerate_after) { st.ramp_type = DECEL_RAMP; }
else if (st.delta_d == current_block->nominal_rate) { st.ramp_type = CRUISE_RAMP; }
else { st.ramp_type = ACCEL_RAMP; }
// Initialize direction bits for block
st.out_bits = pl_current_block->direction_bits ^ settings.invert_mask;
st.execute_step = true; // Set flag to set direction bits upon next ISR tick.
// Initialize Bresenham line counters
st.counter_x = (pl_current_block->step_event_count >> 1);
st.counter_y = st.counter_x;
st.counter_z = st.counter_x;
// Initialize inverse time and step rate counter data
st.counter_d = st_current_data->d_next; // d_next always greater than delta_d.
// During feed hold, do not update rate or ramp type. Keep decelerating.
// if (sys.state == STATE_CYCLE) {
st.delta_d = st_current_data->initial_rate;
// if (st.delta_d == st_current_data->nominal_rate) {
// st.ramp_type = RAMP_NOOP_CRUISE;
st.ramp_type = RAMP_ACCEL;
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid
// }
// }
if (st.delta_d < MINIMUM_STEP_RATE) { st.d_per_tick = MINIMUM_STEP_RATE; }
else { st.d_per_tick = st.delta_d; }
}
// Acceleration and cruise handled by ramping. Just check for deceleration.
if (st_current_segment->flag == ST_DECEL || st_current_segment->flag == ST_DECEL_EOB) {
if (st.ramp_type == RAMP_NOOP_CRUISE) {
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid
} else {
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; // Set ramp counter for triangle
}
st.ramp_type = RAMP_DECEL;
}
st.load_flag = LOAD_NOOP; // Motion loaded. Set no-operation flag until complete.
} else {
// Can't discard planner block here if a feed hold stops in middle of block.
st_go_idle();
bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end
return; // Nothing to do but exit.
}
}
// Adjust inverse time counter for ac/de-celerations
if (st.ramp_type) {
// Tick acceleration ramp counter
st.ramp_count--;
if (st.ramp_count == 0) {
st.ramp_count--; // Tick acceleration ramp counter
if (st.ramp_count == 0) { // Adjust step rate when its time
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter
if (st.ramp_type == ACCEL_RAMP) { // Adjust velocity for acceleration
st.delta_d += current_block->rate_delta;
if (st.delta_d >= current_block->nominal_rate) { // Reached cruise state.
st.ramp_type = CRUISE_RAMP;
st.delta_d = current_block->nominal_rate; // Set cruise velocity
if (st.ramp_type == RAMP_ACCEL) { // Adjust velocity for acceleration
st.delta_d += st_current_data->rate_delta;
if (st.delta_d >= st_current_data->nominal_rate) { // Reached nominal rate.
st.delta_d = st_current_data->nominal_rate; // Set cruising velocity
st.ramp_type = RAMP_NOOP_CRUISE; // Set ramp flag to ignore
}
} else if (st.ramp_type == DECEL_RAMP) { // Adjust velocity for deceleration
if (st.delta_d > current_block->rate_delta) {
st.delta_d -= current_block->rate_delta;
} else { // Adjust velocity for deceleration
if (st.delta_d > st_current_data->rate_delta) {
st.delta_d -= st_current_data->rate_delta;
} else {
// Moving near zero feed rate. Gracefully slow down.
st.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow.
// Check for and handle feed hold exit? At this point, machine is stopped.
}
}
// Finalize adjusted step rate. Ensure minimum.
if (st.delta_d < MINIMUM_STEP_RATE) { st.d_per_tick = MINIMUM_STEP_RATE; }
else { st.d_per_tick = st.delta_d; }
}
}
// Iterate Pramod Ranade inverse time counter. Triggers each Bresenham step event.
if (st.delta_d < MINIMUM_STEP_RATE) { st.d_counter -= MINIMUM_STEP_RATE; }
else { st.d_counter -= st.delta_d; }
// Iterate inverse time counter. Triggers each Bresenham step event.
st.counter_d -= st.d_per_tick;
// Execute Bresenham step event, when it's time to do so.
if (st.d_counter < 0) {
st.d_counter += current_block->d_next;
// Check for feed hold state and execute accordingly.
if (sys.state == STATE_HOLD) {
if (st.ramp_type != DECEL_RAMP) {
st.ramp_type = DECEL_RAMP;
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2;
}
if (st.delta_d <= current_block->rate_delta) {
st_go_idle();
bit_true(sys.execute,EXEC_CYCLE_STOP);
return;
}
}
// TODO: Vary Bresenham resolution for smoother motions or enable faster step rates (>20kHz).
out_bits = current_block->direction_bits; // Reset out_bits and reload direction bits
if (st.counter_d < 0) {
st.counter_d += st_current_data->d_next; // Reload inverse time counter
st.out_bits = pl_current_block->direction_bits; // Reset out_bits and reload direction bits
st.execute_step = true;
// Execute step displacement profile by Bresenham line algorithm
st.counter_x -= current_block->steps_x;
st.counter_x -= pl_current_block->steps[X_AXIS];
if (st.counter_x < 0) {
out_bits |= (1<<X_STEP_BIT);
st.counter_x += st.event_count;
if (out_bits & (1<<X_DIRECTION_BIT)) { sys.position[X_AXIS]--; }
st.out_bits |= (1<<X_STEP_BIT);
st.counter_x += pl_current_block->step_event_count;
if (st.out_bits & (1<<X_DIRECTION_BIT)) { sys.position[X_AXIS]--; }
else { sys.position[X_AXIS]++; }
}
st.counter_y -= current_block->steps_y;
st.counter_y -= pl_current_block->steps[Y_AXIS];
if (st.counter_y < 0) {
out_bits |= (1<<Y_STEP_BIT);
st.counter_y += st.event_count;
if (out_bits & (1<<Y_DIRECTION_BIT)) { sys.position[Y_AXIS]--; }
st.out_bits |= (1<<Y_STEP_BIT);
st.counter_y += pl_current_block->step_event_count;
if (st.out_bits & (1<<Y_DIRECTION_BIT)) { sys.position[Y_AXIS]--; }
else { sys.position[Y_AXIS]++; }
}
st.counter_z -= current_block->steps_z;
st.counter_z -= pl_current_block->steps[Z_AXIS];
if (st.counter_z < 0) {
out_bits |= (1<<Z_STEP_BIT);
st.counter_z += st.event_count;
if (out_bits & (1<<Z_DIRECTION_BIT)) { sys.position[Z_AXIS]--; }
st.out_bits |= (1<<Z_STEP_BIT);
st.counter_z += pl_current_block->step_event_count;
if (st.out_bits & (1<<Z_DIRECTION_BIT)) { sys.position[Z_AXIS]--; }
else { sys.position[Z_AXIS]++; }
}
// Check step events for trapezoid change or end of block.
st.step_events_remaining--; // Decrement step events count
if (st.step_events_remaining) {
if (st.ramp_type != DECEL_RAMP) {
// Acceleration and cruise handled by ramping. Just check for deceleration.
if (st.step_events_remaining <= current_block->decelerate_after) {
st.ramp_type = DECEL_RAMP;
if (st.step_events_remaining == current_block->decelerate_after) {
if (st.delta_d == current_block->nominal_rate) {
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid
} else {
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; // Set ramp counter for triangle
}
}
}
st.segment_steps_remaining--; // Decrement step events count
if (st.segment_steps_remaining == 0) {
// Line move is complete, set load line flag to check for new move.
// Check if last line move in planner block. Discard if so.
if (st_current_segment->flag == ST_END_OF_BLOCK || st_current_segment->flag == ST_DECEL_EOB) {
plan_discard_current_block();
st.load_flag = LOAD_BLOCK;
} else {
st.load_flag = LOAD_LINE;
}
} else {
// If current block is finished, reset pointer
current_block = NULL;
plan_discard_current_block();
// Discard current segment
segment_buffer_tail = next_block_index( segment_buffer_tail );
// NOTE: sys.position updates could be done here. The bresenham counters can have
// their own fast 8-bit addition-only counters. Here we would check the direction and
// apply it to sys.position accordingly. However, this could take too much time.
}
out_bits ^= settings.invert_mask; // Apply step port invert mask
st.out_bits ^= settings.invert_mask; // Apply step port invert mask
}
busy = false;
// SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT;
@ -314,8 +396,14 @@ ISR(TIMER0_OVF_vect)
void st_reset()
{
memset(&st, 0, sizeof(st));
current_block = NULL;
pl_current_block = NULL;
pl_prep_block = NULL;
pl_prep_index = 0;
st_data_prep_index = 0;
st.load_flag = LOAD_BLOCK;
busy = false;
segment_buffer_tail = 0;
segment_next_head = 1;
}
@ -374,14 +462,179 @@ void st_feed_hold()
// Only the planner de/ac-celerations profiles and stepper rates have been updated.
void st_cycle_reinitialize()
{
if (current_block != NULL) {
// if (pl_current_block != NULL) {
// Replan buffer from the feed hold stop location.
plan_cycle_reinitialize(st.step_events_remaining);
st.ramp_type = ACCEL_RAMP;
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2;
st.delta_d = 0;
sys.state = STATE_QUEUED;
} else {
// TODO: Need to add up all of the step events in the current planner block to give
// back to the planner. Should only need it for the current block.
// BUT! The planner block millimeters is all changed and may be changed into the next
// planner block. The block millimeters would need to be recalculated via step counts
// and the mm/step variable.
// OR. Do we plan the feed hold itself down with the planner.
// plan_cycle_reinitialize(st_current_data->step_events_remaining);
// st.ramp_type = RAMP_ACCEL;
// st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2;
// st.delta_d = 0;
// sys.state = STATE_QUEUED;
// } else {
// sys.state = STATE_IDLE;
// }
sys.state = STATE_IDLE;
}
}
/* Preps stepper buffer. Called from main program.
NOTE: There doesn't seem to be a great way to figure out how many steps occur within
a set number of ISR ticks. Numerical round-off and CPU overhead always seems to be a
critical problem. So, either numerical round-off checks could be made to account for
them, while CPU overhead could be minimized in some way, or we can flip the algorithm
around to have the stepper algorithm track number of steps over an indeterminant amount
of time instead.
In other words, we use the planner velocity floating point data to get an estimate of
the number of steps we want to execute. We then back out the approximate velocity for
the planner to use, which should be much more robust to round-off error. The main problem
now is that we are loading the stepper algorithm to handle acceleration now, rather than
pre-calculating with the main program. This approach does make sense in the way that
planner velocities and stepper profiles can be traced more accurately.
Which is better? Very hard to tell. The time-based algorithm would be able to handle
Bresenham step adaptive-resolution much easier and cleaner. Whereas, the step-based would
require some additional math in the stepper algorithm to adjust on the fly, plus adaptation
would occur in a non-deterministic manner.
I suppose it wouldn't hurt to build both to see what's better. Just a lot more work.
TODO: Need to describe the importance of continuations of step pulses between ramp states
and planner blocks. This has to do with Alden's problem with step "phase". The things I've
been doing here limit this phase issue by truncating some of the ramp timing for certain
events like deceleration initialization and end of block.
*/
void st_prep_buffer()
{
while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer.
// Determine if we need to load a new planner block.
if (pl_prep_block == NULL) {
pl_prep_block = plan_get_block_by_index(pl_prep_index);
if (pl_prep_block == NULL) { return; } // No more planner blocks. Let stepper finish out.
// Prepare commonly shared planner block data for the ensuing step buffer moves
st_data_prep_index = next_block_index(st_data_prep_index);
st_prep_data = &segment_data[st_data_prep_index];
// Initialize Bresenham variables
st_prep_data->step_events_remaining = pl_prep_block->step_event_count;
// Convert new block to stepper variables.
// NOTE: This data can change mid-block from normal planner updates and feedrate overrides. Must
// be maintained as these execute.
// TODO: If the planner updates this block, particularly from a deceleration to an acceleration,
// we must reload the initial rate data, such that the velocity profile is re-constructed correctly.
st_prep_data->initial_rate = ceil(sqrt(pl_prep_block->entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
// This data doesn't change. Could be performed in the planner, but fits nicely here.
// Although, acceleration can change for S-curves. So keep it here.
st_prep_data->rate_delta = ceil(pl_prep_block->acceleration*
((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic)
// This definitely doesn't change, but could be precalculated in a way to help some of the
// math in this handler, i.e. millimeters per step event data.
st_prep_data->d_next = ceil((pl_prep_block->millimeters*INV_TIME_MULTIPLIER)/pl_prep_block->step_event_count); // (mult*mm/step)
st_prep_data->mm_per_step = pl_prep_block->millimeters/pl_prep_block->step_event_count;
// Calculate trapezoid data from planner.
st_prep_data->decelerate_after = calculate_trapezoid_for_block(pl_prep_index);
}
/*
TODO: Need to check for a planner flag to indicate a change to this planner block.
If so, need to check for a change in acceleration state, from deceleration to acceleration,
to reset the stepper ramp counters and the initial_rate data to trace the new
ac/de-celeration profile correctly.
No change conditions:
- From nominal speed to acceleration from feedrate override
- From nominal speed to new deceleration.
- From acceleration to new deceleration point later or cruising point.
- From acceleration to immediate deceleration? Can happen during feedrate override
and slowing down, but likely ok by enforcing the normal ramp counter protocol.
Change conditions:
- From deceleration to acceleration, i.e. common with jogging when new blocks are added.
*/
st_segment_t *st_prep_segment = &segment_buffer[segment_buffer_head];
st_prep_segment->st_data_index = st_data_prep_index;
// TODO: How do you cheaply compute n_step without a sqrt()? Could be performed as 'bins'.
st_prep_segment->n_step = 250; //floor( (exit_speed*approx_time)/mm_per_step );
// st_segment->n_step = max(st_segment->n_step,MINIMUM_STEPS_PER_BLOCK); // Ensure it moves for very slow motions?
// st_segment->n_step = min(st_segment->n_step,MAXIMUM_STEPS_PER_BLOCK); // Prevent unsigned int8 overflow.
// Check if n_step exceeds steps remaining in planner block. If so, truncate.
if (st_prep_segment->n_step > st_prep_data->step_events_remaining) {
st_prep_segment->n_step = st_prep_data->step_events_remaining;
}
// Check if n_step exceeds decelerate point in block. Need to perform this so that the
// ramp counters are reset correctly in the stepper algorithm. Can be 1 step, but should
// be OK since it is likely moving at a fast rate already.
if (st_prep_data->decelerate_after > 0) {
if (st_prep_segment->n_step > st_prep_data->decelerate_after) {
st_prep_segment->n_step = st_prep_data->decelerate_after;
}
}
// float distance, exit_speed_sqr;
// distance = st_prep_segment->n_step*st_prep_data->mm_per_step; // Always greater than zero
// if (st_prep_data->step_events_remaining >= pl_prep_block->decelerate_after) {
// exit_speed_sqr = pl_prep_block->entry_speed_sqr - 2*pl_prep_block->acceleration*distance;
// } else { // Acceleration or cruising ramp
// if (pl_prep_block->entry_speed_sqr < pl_prep_block->nominal_speed_sqr) {
// exit_speed_sqr = pl_prep_block->entry_speed_sqr + 2*pl_prep_block->acceleration*distance;
// if (exit_speed_sqr > pl_prep_block->nominal_speed_sqr) { exit_speed_sqr = pl_prep_block->nominal_speed_sqr; }
// } else {
// exit_speed_sqr = pl_prep_block->nominal_speed_sqr;
// }
// }
// Update planner block variables.
// pl_prep_block->entry_speed_sqr = max(0.0,exit_speed_sqr);
// pl_prep_block->max_entry_speed_sqr = exit_speed_sqr; // ??? Overwrites the corner speed. May need separate variable.
// pl_prep_block->millimeters -= distance; // Potential round-off error near end of block.
// pl_prep_block->millimeters = max(0.0,pl_prep_block->millimeters); // Shouldn't matter.
// Update stepper block variables.
st_prep_data->step_events_remaining -= st_prep_segment->n_step;
if ( st_prep_data->step_events_remaining == 0 ) {
// Move planner pointer to next block
if (st_prep_data->decelerate_after == 0) {
st_prep_segment->flag = ST_DECEL_EOB;
} else {
st_prep_segment->flag = ST_END_OF_BLOCK;
}
pl_prep_index = next_block_pl_index(pl_prep_index);
pl_prep_block = NULL;
printString("EOB");
} else {
if (st_prep_data->decelerate_after == 0) {
st_prep_segment->flag = ST_DECEL;
} else {
st_prep_segment->flag = ST_NOOP;
}
printString("x");
}
st_prep_data->decelerate_after -= st_prep_segment->n_step;
// New step block completed. Increment step buffer indices.
segment_buffer_head = segment_next_head;
segment_next_head = next_block_index(segment_buffer_head);
printInteger((long)st_prep_segment->n_step);
printString(" ");
printInteger((long)st_prep_data->decelerate_after);
printString(" ");
printInteger((long)st_prep_data->step_events_remaining);
}
}

2
stepper.h
View File

@ -45,4 +45,6 @@ void st_cycle_reinitialize();
// Initiates a feed hold of the running program
void st_feed_hold();
void st_prep_buffer();
#endif

601
stepper_new2.c Normal file
View File

@ -0,0 +1,601 @@
/*
stepper.c - stepper motor driver: executes motion plans using stepper motors
Part of Grbl
Copyright (c) 2011-2013 Sungeun K. Jeon
Copyright (c) 2009-2011 Simen Svale Skogsrud
Grbl is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
Grbl is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with Grbl. If not, see <http://www.gnu.org/licenses/>.
*/
#include <avr/interrupt.h>
#include "stepper.h"
#include "config.h"
#include "settings.h"
#include "planner.h"
#include "nuts_bolts.h"
// Some useful constants
#define TICKS_PER_MICROSECOND (F_CPU/1000000)
#define CRUISE_RAMP 0
#define ACCEL_RAMP 1
#define DECEL_RAMP 2
#define LOAD_NOOP 0
#define LOAD_LINE 1
#define LOAD_BLOCK 2
// Stepper state variable. Contains running data and trapezoid variables.
typedef struct {
// Used by the bresenham line algorithm
int32_t counter_x, // Counter variables for the bresenham line tracer
counter_y,
counter_z;
int32_t step_events_remaining; // Steps remaining in line motion
// Used by inverse time algorithm
int32_t delta_d; // Inverse time distance traveled per interrupt tick
int32_t d_counter; // Inverse time distance traveled since last step event
int32_t d_per_tick;
// Used by the stepper driver interrupt
uint8_t execute_step; // Flags step execution for each interrupt.
uint8_t step_pulse_time; // Step pulse reset time after step rise
uint8_t out_bits; // The next stepping-bits to be output
uint8_t load_flag;
} stepper_t;
static stepper_t st;
#define STEPPER_BUFFER_SIZE 5
typedef struct {
int32_t event_count;
int32_t rate;
uint8_t end_of_block;
uint8_t tick_count;
int32_t initial_rate; // The step rate at start of block
int32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive)
int32_t decelerate_after; // The index of the step event on which to start decelerating
int32_t nominal_rate; // The nominal step rate for this block in step_events/minute
int32_t d_next; // Scaled distance to next step
} stepper_buffer_t;
static stepper_buffer_t step_buffer[STEPPER_BUFFER_SIZE];
static volatile uint8_t step_buffer_tail;
static uint8_t step_buffer_head;
static uint8_t step_next_head;
// NOTE: If the main interrupt is guaranteed to be complete before the next interrupt, then
// this blocking variable is no longer needed. Only here for safety reasons.
static volatile uint8_t busy; // True when "Stepper Driver Interrupt" is being serviced. Used to avoid retriggering that handler.
static plan_block_t *plan_current_block; // A pointer to the planner block currently being traced
/* __________________________
/| |\ _________________ ^
/ | | \ /| |\ |
/ | | \ / | | \ s
/ | | | | | \ p
/ | | | | | \ e
+-----+------------------------+---+--+---------------+----+ e
| BLOCK 1 | BLOCK 2 | d
time ----->
The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta
until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after
after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as
+/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND.
*/
// Stepper state initialization. Cycle should only start if the st.cycle_start flag is
// enabled. Startup init and limits call this function but shouldn't start the cycle.
void st_wake_up()
{
// Enable steppers by resetting the stepper disable port
if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) {
STEPPERS_DISABLE_PORT |= (1<<STEPPERS_DISABLE_BIT);
} else {
STEPPERS_DISABLE_PORT &= ~(1<<STEPPERS_DISABLE_BIT);
}
if (sys.state == STATE_CYCLE) {
// Initialize stepper output bits
st.out_bits = settings.invert_mask;
// Initialize step pulse timing from settings.
st.step_pulse_time = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3);
// Enable stepper driver interrupt
st.execute_step = false;
TCNT2 = 0; // Clear Timer2
TIMSK2 |= (1<<OCIE2A); // Enable Timer2 Compare Match A interrupt
TCCR2B = (1<<CS21); // Begin Timer2. Full speed, 1/8 prescaler
}
}
// Stepper shutdown
void st_go_idle()
{
// Disable stepper driver interrupt. Allow Timer0 to finish. It will disable itself.
TIMSK2 &= ~(1<<OCIE2A); // Disable Timer2 interrupt
TCCR2B = 0; // Disable Timer2
busy = false;
// Disable steppers only upon system alarm activated or by user setting to not be kept enabled.
if ((settings.stepper_idle_lock_time != 0xff) || bit_istrue(sys.execute,EXEC_ALARM)) {
// Force stepper dwell to lock axes for a defined amount of time to ensure the axes come to a complete
// stop and not drift from residual inertial forces at the end of the last movement.
delay_ms(settings.stepper_idle_lock_time);
if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) {
STEPPERS_DISABLE_PORT &= ~(1<<STEPPERS_DISABLE_BIT);
} else {
STEPPERS_DISABLE_PORT |= (1<<STEPPERS_DISABLE_BIT);
}
}
}
/* "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Grbl. It is based
on the Pramod Ranade inverse time stepper algorithm, where a timer ticks at a constant
frequency and uses time-distance counters to track when its the approximate time for any
step event. However, the Ranade algorithm, as described, is susceptible to numerical round-off,
meaning that some axes steps may not execute for a given multi-axis motion.
Grbl's algorithm slightly differs by using a single Ranade time-distance counter to manage
a Bresenham line algorithm for multi-axis step events which ensures the number of steps for
each axis are executed exactly. In other words, it uses a Bresenham within a Bresenham algorithm,
where one tracks time(Ranade) and the other steps.
This interrupt pops blocks from the block_buffer and executes them by pulsing the stepper pins
appropriately. It is supported by The Stepper Port Reset Interrupt which it uses to reset the
stepper port after each pulse. The bresenham line tracer algorithm controls all three stepper
outputs simultaneously with these two interrupts.
*/
// NOTE: Average time in this ISR is: 5 usec iterating timers only, 20-25 usec with step event, or
// 15 usec when popping a block. So, ensure Ranade frequency and step pulse times work with this.
ISR(TIMER2_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
// Pulse stepper port pins, if flagged. New block dir will always be set one timer tick
// before any step pulse due to algorithm design.
if (st.execute_step) {
st.execute_step = false;
STEPPING_PORT = ( STEPPING_PORT & ~(DIRECTION_MASK | STEP_MASK) ) | st.out_bits;
TCNT0 = st.step_pulse_time; // Reload Timer0 counter.
TCCR0B = (1<<CS21); // Begin Timer0. Full speed, 1/8 prescaler
}
busy = true;
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.
// This loading step is important. Allows the stepper ISR to only handle its own variables
// hence no volatiles needed. Otherwise a preloading step is required by the main program
// or some other means to get the line motions started, and volatile would be required.
// If there is no current block, attempt to pop one from the buffer
if (st.load_flag != LOAD_NOOP) {
// Anything in the buffer? If so, initialize next motion.
if (step_buffer_head != step_buffer_tail) {
// NOTE: Loads after a step event. At high rates above 1/2 ISR frequency, there is
// a small chance that this will load at the same time as a step event. Hopefully,
// the overhead for this loading event isn't too much.. possibly 2-5 usec.
// NOTE: The stepper algorithm must control the planner buffer tail as it completes
// the block moves. Otherwise, a feed hold can leave a few step buffer line moves
// without the correct planner block information.
// Load line motion from stepper buffer
st.step_events_remaining = step_buffer[step_buffer_tail].event_count;
st.delta_d = step_buffer[step_buffer_tail].rate;
// Check if the counters need to be reset
if (st.load_flag == LOAD_BLOCK) {
plan_current_block = plan_get_current_block();
// Initialize direction bits for block
st.out_bits = plan_current_block->direction_bits ^ settings.invert_mask;
st.execute_step = true; // Set flag to set direction bits.
st.counter_x = (plan_current_block->step_event_count >> 1);
st.counter_y = st.counter_x;
st.counter_z = st.counter_x;
// This is correct. Sets the total time before the next step occurs.
st.counter_d = plan_current_block->d_next; // d_next always greater than delta_d.
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid
}
st.load_flag = LOAD_NOOP; // Line motion loaded. Set no-operation flag until complete.
} else {
// Can't discard planner block here if a feed hold stops in middle of block.
st_go_idle();
bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end
return; // Nothing to do but exit.
}
}
// Iterate inverse time counter. Triggers each Bresenham step event.
st.counter_d -= st.delta_d;
// Execute Bresenham step event, when it's time to do so.
if (st.counter_d < 0) {
st.counter_d += plan_current_block->d_next; // Reload inverse time counter
st.out_bits = plan_current_block->direction_bits; // Reset out_bits and reload direction bits
st.execute_step = true;
// Execute step displacement profile by Bresenham line algorithm
st.counter_x -= plan_current_block->steps[X_AXIS];
if (st.counter_x < 0) {
st.out_bits |= (1<<X_STEP_BIT);
st.counter_x += plan_current_block->step_event_count;
if (st.out_bits & (1<<X_DIRECTION_BIT)) { sys.position[X_AXIS]--; }
else { sys.position[X_AXIS]++; }
}
st.counter_y -= plan_current_block->steps[Y_AXIS];
if (st.counter_y < 0) {
st.out_bits |= (1<<Y_STEP_BIT);
st.counter_y += plan_current_block->step_event_count;
if (st.out_bits & (1<<Y_DIRECTION_BIT)) { sys.position[Y_AXIS]--; }
else { sys.position[Y_AXIS]++; }
}
st.counter_z -= plan_current_block->steps[Z_AXIS];
if (st.counter_z < 0) {
st.out_bits |= (1<<Z_STEP_BIT);
st.counter_z += plan_current_block->step_event_count;
if (st.out_bits & (1<<Z_DIRECTION_BIT)) { sys.position[Z_AXIS]--; }
else { sys.position[Z_AXIS]++; }
}
// Check step events for trapezoid change or end of block.
st.step_events_remaining--; // Decrement step events count
// Tracking step events may not be the right thing. Need to track time instead.
// If a step completes and the inverse counters are reset, then the line motion time
// to execute gets truncated. Thus, screwing up the acceleration.
// Time can change at the first block, a transition point or end of block. It is not constant.
// Time must be truncated when the last step in the block is executed. Ensures there are
// no step phase issues with the following block. Or, the ISR timer count must be computed
// to be exact, such that the last step occurs correctly.
// This should always/automatically occur for a true trapezoid. The first acceleration
// should be 1/2 accel_tick and the last decel should also. The difference is taken up by
// the transition from cruise to decel.
// For a cruising entry, the transition block takes care of incompatibility.
// For a cruising exit, the exit block needs to truncate. Especially when the cruising
// continues onward to the next block.
if (st.step_events_remaining == 0) {
// Line move is complete, set load line flag to check for new move.
// Check if last line move in planner block. Discard if so.
if (step_buffer[step_buffer_tail].end_of_block) {
plan_discard_current_block();
st.load_flag = LOAD_BLOCK;
} else {
st.load_flag = LOAD_LINE;
}
// Discard current block
if (step_buffer_head != step_buffer_tail) {
step_buffer_tail = next_block_index( step_buffer_tail );
}
// NOTE: sys.position updates could be done here. The bresenham counters can have
// their own incremental counters. Here we would check the direction and apply it
// to sys.position accordingly. However, this could take some time.
}
st.out_bits ^= settings.invert_mask; // Apply step port invert mask
}
busy = false;
// SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT;
}
// The Stepper Port Reset Interrupt: Timer0 OVF interrupt handles the falling edge of the
// step pulse. This should always trigger before the next Timer2 COMPA interrupt and independently
// finish, if Timer2 is disabled after completing a move.
ISR(TIMER0_OVF_vect)
{
STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | (settings.invert_mask & STEP_MASK);
TCCR0B = 0; // Disable timer until needed.
}
// Reset and clear stepper subsystem variables
void st_reset()
{
memset(&st, 0, sizeof(st));
st.load_flag = LOAD_BLOCK;
busy = false;
}
// Initialize and start the stepper motor subsystem
void st_init()
{
// Configure directions of interface pins
STEPPING_DDR |= STEPPING_MASK;
STEPPING_PORT = (STEPPING_PORT & ~STEPPING_MASK) | settings.invert_mask;
STEPPERS_DISABLE_DDR |= 1<<STEPPERS_DISABLE_BIT;
// Configure Timer 2
TIMSK2 &= ~(1<<OCIE2A); // Disable Timer2 interrupt while configuring it
TCCR2B = 0; // Disable Timer2 until needed
TCNT2 = 0; // Clear Timer2 counter
TCCR2A = (1<<WGM21); // Set CTC mode
OCR2A = (F_CPU/ISR_TICKS_PER_SECOND)/8 - 1; // Set Timer2 CTC rate
// Configure Timer 0
TIMSK0 &= ~(1<<TOIE0);
TCCR0A = 0; // Normal operation
TCCR0B = 0; // Disable Timer0 until needed
TIMSK0 |= (1<<TOIE0); // Enable overflow interrupt
// Start in the idle state, but first wake up to check for keep steppers enabled option.
st_wake_up();
st_go_idle();
}
// Planner external interface to start stepper interrupt and execute the blocks in queue. Called
// by the main program functions: planner auto-start and run-time command execution.
void st_cycle_start()
{
if (sys.state == STATE_QUEUED) {
sys.state = STATE_CYCLE;
st_wake_up();
}
}
// Execute a feed hold with deceleration, only during cycle. Called by main program.
void st_feed_hold()
{
if (sys.state == STATE_CYCLE) {
sys.state = STATE_HOLD;
sys.auto_start = false; // Disable planner auto start upon feed hold.
}
}
// Reinitializes the cycle plan and stepper system after a feed hold for a resume. Called by
// runtime command execution in the main program, ensuring that the planner re-plans safely.
// NOTE: Bresenham algorithm variables are still maintained through both the planner and stepper
// cycle reinitializations. The stepper path should continue exactly as if nothing has happened.
// Only the planner de/ac-celerations profiles and stepper rates have been updated.
void st_cycle_reinitialize()
{
if (current_block != NULL) {
// Replan buffer from the feed hold stop location.
plan_cycle_reinitialize(st.step_events_remaining);
st.ramp_type = ACCEL_RAMP;
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2;
st.delta_d = 0;
sys.state = STATE_QUEUED;
} else {
sys.state = STATE_IDLE;
}
}
// Preps stepper buffer. Called from main program. Sends short line segments of constant
// velocity to the stepper driver. Ac/de-celeration calculations are performed here.
// NOTE: Line motions can range from 1-300
// TODO: Could be very easy to install adaptive Bresenham resolution with this prep function.\
That is, if the acceleration calculations are performed here rather than in the ISR.
void st_prep_buffer()
{
while (st.buffer_tail != st.next_head) { // Check if we need to fill the buffer.
step_block_t *step_block = &step_buffer[st.buffer_head];
// Determine if we need to load a new planner block.
if (plan_current_block == NULL) {
plan_current_block = plan_get_current_block();
if (plan_current_block == NULL) { return; } // No more planner blocks. Let stepper finish out.
// Initialize Bresenham variables
step_buffer[st.buffer_head] = plan_current_block->step_event_count;
//st.step_events_remaining = st.event_count;
// Convert new block to stepper variables.
// NOTE: This data can change mid-block from normal planner updates and feedrate overrides. Must
// be maintained as these execute.
// TODO: The initial rate needs to be sent back to the planner to update the entry speed
block->initial_rate = ceil(sqrt(plan_current_block->entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
block->nominal_rate = ceil(plan_current_block->nominal_speed*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
// This data doesn't change. Could be performed in the planner, but fits nicely here.
// Although, acceleration can change for S-curves. So keep it here.
block->rate_delta = ceil(plan_current_block->acceleration*
((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic)
// This definitely doesn't change, but could be precalculated in a way to help some of the
// math in this handler, i.e. millimeters per step event data.
block->d_next = ceil((plan_current_block->millimeters*INV_TIME_MULTIPLIER)/plan_current_block->step_event_count); // (mult*mm/step)
// During feed hold, do not update Ranade counter, rate, or ramp type. Keep decelerating.
if (sys.state == STATE_CYCLE) { }
}
// Track instead the trapezoid line and use the average of the entry and exit velocities
// to determine step rate. This should take care of the deceleration issue automatically...
// i think.
// First need to figure out what type of profile is segment is, i.e. acceleration only, accel
// to decel triangle, cruise to decel, or all three. May need more profile data to compute this
// from the planner itself, like accelerate until.
// Another issue. This is only tracking the velocity profile, not the distance covered over that
// time period. This can lead to an unsynchronized velocity profile and steps executed. But,
// how much drift is there really? Enough to be a problem? Not sure. I would think typical
// drift would be on the order of a few steps, more depending on step resolution.
entry_rate = last_exit_rate;
time = 250 ISR ticks per acceleration tick.
distance = 0;
if (distance_traveled < accelerate_until)
exit_rate = entry_rate + acceleration*time;
if (exit_rate > nominal_rate) {
exit_rate = nominal_rate;
time = 2*(accelerate_until-distance_travel)/(entry_rate+nominal_rate);
// distance = accelerate_until; // Enforce distance?
// Truncate this segment.
}
} else if (distance_traveled >= decelerate_after) {
if (accelerate_until == decelerate_after) {
time = last time;
exit_rate = entry_rate;
} else {
exit_rate = entry_rate - acceleration*time;
}
} else {
exit_rate = nominal_rate; // Just cruise
distance = nominal_rate*time;
if (distance > decelerate_after) { // Truncate segment at nominal rate.
time = (decelerate_after-distance_traveled)/(nominal_rate);
distance = decelerate_after;
}
}
mean_rate = 0.5*(entry_rate+exit_rate);
distance = mean_rate*time;
if (entry_rate < nominal_rate) {
if (entry_distance < decelerate_after) { // Acceleration case
exit_rate = entry_rate + acceleration*time
exit_rate = min(exit_rate,nominal_rate);
mean_rate = 0.5*(entry_rate + exit_rate);
distance = mean_rate*time;
if (distance > decelerate_after) {
exit_rate =
// If the MINIMUM_STEP_RATE is less than ACCELERATION_TICKS_PER_SECOND then there can be
// rate adjustements that have less than one step per tick.
// How do you deal with the remainer?
time = 250 ISR ticks per acceleration tick. (30000/120)
delta_d*time // mm per acceleration tick
delta_d*time/d_next // number of steps/acceleration_tick. Chance of integer overflow.
delta_d*time/d_next + last_remainder. // steps/acceleration_tick.
n_step*d_next/delta_d // number of ISR ticks for enforced n_steps.
// In floating point? Then convert?
// Requires exact millimeters. Roundoff might be a problem. But could be corrected by just
// checking if the total step event counts are performed.
// Could be limited by float conversion and about 1e7 steps per block.
line_mm = feed_rate / acc_tick // mm per acc_tick
n_steps = floor(line_mm * step_event_remaining/millimeters_remaining) // steps. float 7.2 digits|int32 10 digits
millimeters_remaining -= line_mm;
step_events_remaining -= n_steps;
// There doesn't seem to be a way to avoid this divide here.
line_mm = feed_rate / acc_tick // mm per acc_tick
n_steps = floor( (line_mm+line_remainder) * step_event_count/millimeters) // steps. float 7.2 digits|int32 10 digits
line_remainder = line_mm - n_steps*(millimeters/step_event_count);
// Need to handle when rate is very very low, i.e. less than one step per accel tick.
// Could be bounded by MINIMUM_STEP_RATE.
// 1. Figure out how many steps occur exactly within n ISR ticks.
// 2. Account for step-time remainder for next line motion exactly.
// 3. At the end of block, determine exact number of ISR ticks to finish the steps. Or,\
have the ISR track steps to exit on time. It would require an extra counter.
// NOTE: There doesn't seem to be a great way to figure out how many steps occur within
// a set number of ISR ticks. Numerical round-off and CPU overhead always seems to be a
// critical problem. So, either numerical round-off checks could be made to account for
// them, while CPU overhead could be minimized in some way, or we can flip the algorithm
// around to have the stepper algorithm track number of steps over an indeterminant amount
// of time instead.
// In other words, we use the planner velocity floating point data to get an estimate of
// the number of steps we want to execute. We then back out the approximate velocity for
// the planner to use, which should be much more robust to round-off error. The main problem
// now is that we are loading the stepper algorithm to handle acceleration now, rather than
// pre-calculating with the main program. This approach does make sense in the way that
// planner velocities and stepper profiles can be traced more accurately.
// Which is better? Very hard to tell. The time-based algorithm would be able to handle
// Bresenham step adaptive-resolution much easier and cleaner. Whereas, the step-based would
// require some additional math in the stepper algorithm to adjust on the fly, plus adaptation
// would occur in a non-deterministic manner.
// I suppose it wouldn't hurt to build both to see what's better. Just a lot more work.
feed_rate/120 = millimeters per acceleration tick
steps?
d_next // (mult*mm/step)
rate // (mult*mm/isr_tic)
rate/d_next // step/isr_tic
if (plan_current_block->step_events_remaining <= plan_current_block->decelerate_after) {
// Determine line segment velocity and associated inverse time counter.
if (step_block.ramp_type == ACCEL_RAMP) { // Adjust velocity for acceleration
step_block.delta_d += plan_current_block->rate_delta;
if (step_block.delta_d >= plan_current_block->nominal_rate) { // Reached cruise state.
step_block.ramp_type = CRUISE_RAMP;
step_block.delta_d = plan_current_block->nominal_rate; // Set cruise velocity
}
}
} else { // Adjust velocity for deceleration
if (step_block.delta_d > plan_current_block->rate_delta) {
step_block.delta_d -= plan_current_block->rate_delta;
} else {
step_block.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow.
}
}
// Incorrect. Can't overwrite delta_d. Needs to override instead.
if (step_block.delta_d < MINIMUM_STEP_RATE) { step_block.delta_d = MINIMUM_STEP_RATE; }
/* - Compute the number of steps needed to complete this move over the move time, i.e.
ISR_TICKS_PER_ACCELERATION_TICK.
- The first block in the buffer is half of the move time due to midpoint rule.
- Check if this reaches the deceleration after location. If so, truncate move. Also,
if this is a triangle move, double the truncated move to stay with midpoint rule.
NOTE: This can create a stepper buffer move down to just one step in length.
- Update the planner block entry speed for the planner to compute from end of the
stepper buffer location.
- If a feed hold occurs, begin to enforce deceleration, while enforcing the above rules.
When the deceleration is complete, all we need to do is update the planner block
entry speed and force a replan.
*/
// Planner block move completed.
// TODO: planner buffer tail no longer needs to be volatile. only accessed by main program.
if (st.step_events_remaining == 0) {
plan_current_block = NULL; // Set flag that we are done with this planner block.
plan_discard_current_block();
}
step_buffer_head = step_next_head;
step_next_head = next_block_index(step_buffer_head);
}
}

31
stepper_new.c → stepper_new_dual_ISR.c
View File

@ -34,9 +34,7 @@
// Stepper state variable. Contains running data and trapezoid variables.
typedef struct {
// Used by the bresenham line algorithm
int32_t counter_x, // Counter variables for the bresenham line tracer
counter_y,
counter_z;
int32_t counter[N_AXIS]; // Counter variables for the bresenham line tracer
uint32_t event_count; // Total event count. Retained for feed holds.
uint32_t step_events_remaining; // Steps remaining in motion
@ -182,26 +180,29 @@ won't take too much time in the interrupt.
// Prepare Bresenham step event, when it's time to do so.
if (st.d_counter < 0) {
st.d_counter += current_block->d_next;
// Load next step
out_bits = current_block->direction_bits; // Reset out_bits and reload direction bits
st.execute_step = true;
// Configure next step
out_bits = current_block->direction_bits; // Reset out_bits and reload direction bits
// Execute step displacement profile by Bresenham line algorithm
st.counter_x -= current_block->steps_x;
if (st.counter_x < 0) {
st.counter[X_AXIS] -= current_block->steps_x; // Doesn't change when set up.
if (st.counter[X_AXIS] < 0) {
out_bits |= (1<<X_STEP_BIT);
st.counter_x += st.event_count;
st.counter[X_AXIS] += st.event_count;
st.n_step[X_AXIS]; // Track number of steps
}
st.counter_y -= current_block->steps_y;
if (st.counter_y < 0) {
st.counter[Y_AXIS] -= current_block->steps_y;
if (st.counter[Y_AXIS] < 0) {
out_bits |= (1<<Y_STEP_BIT);
st.counter_y += st.event_count;
st.counter[Y_AXIS] += st.event_count;
st.n_step[Y_AXIS]++;
}
st.counter_z -= current_block->steps_z;
if (st.counter_z < 0) {
st.counter[Z_AXIS] -= current_block->steps_z;
if (st.counter[Z_AXIS] < 0) {
out_bits |= (1<<Z_STEP_BIT);
st.counter_z += st.event_count;
st.counter[Z_AXIS] += st.event_count;
st.n_step[Z_AXIS]++;
}
// Check step events for trapezoid change or end of block.

680
stepper_new_time.c Normal file
View File

@ -0,0 +1,680 @@
/*
stepper.c - stepper motor driver: executes motion plans using stepper motors
Part of Grbl
Copyright (c) 2011-2013 Sungeun K. Jeon
Copyright (c) 2009-2011 Simen Svale Skogsrud
Grbl is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
Grbl is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with Grbl. If not, see <http://www.gnu.org/licenses/>.
*/
#include <avr/interrupt.h>
#include "stepper.h"
#include "config.h"
#include "settings.h"
#include "planner.h"
#include "nuts_bolts.h"
// Some useful constants
#define TICKS_PER_MICROSECOND (F_CPU/1000000)
#define RAMP_NOOP_CRUISE 0
#define RAMP_ACCEL 1
#define RAMP_DECEL 2
#define LOAD_NOOP 0
#define LOAD_LINE 1
#define LOAD_BLOCK 2
#define ST_NOOP 0
#define ST_END_OF_BLOCK 1
// Stepper state variable. Contains running data and trapezoid variables.
typedef struct {
// Used by the bresenham line algorithm
int32_t counter_x, // Counter variables for the bresenham line tracer
counter_y,
counter_z;
int8_t segment_steps_remaining; // Steps remaining in line motion
// Used by inverse time algorithm to track step rate
int32_t counter_d; // Inverse time distance traveled since last step event
int32_t delta_d; // Inverse time distance traveled per interrupt tick
int32_t d_per_tick;
// Used by the stepper driver interrupt
uint8_t execute_step; // Flags step execution for each interrupt.
uint8_t step_pulse_time; // Step pulse reset time after step rise
uint8_t out_bits; // The next stepping-bits to be output
uint8_t load_flag;
uint8_t ramp_count;
uint8_t ramp_type;
} stepper_t;
static stepper_t st;
#define SEGMENT_BUFFER_SIZE 5
// Stores stepper buffer common data. Can change planner mid-block in special conditions.
typedef struct {
int32_t step_events_remaining; // Tracks step event count for the executing planner block
int32_t d_next; // Scaled distance to next step
float mm_per_step;
} st_data_t;
static st_data_t segment_data[SEGMENT_BUFFER_SIZE];
// Primary stepper motion buffer
typedef struct {
uint8_t n_step;
int32_t rate;
uint8_t st_data_index;
uint8_t flag;
} st_segment_t;
static st_segment_t segment_buffer[SEGMENT_BUFFER_SIZE];
static volatile uint8_t segment_buffer_tail;
static uint8_t segment_buffer_head;
static uint8_t segment_next_head;
static volatile uint8_t busy; // Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though.
static plan_block_t *pl_current_block; // A pointer to the planner block currently being traced
static st_segment_t *st_current_segment;
static st_data_t *st_current_data;
static plan_block_t *pl_prep_block; // A pointer to the planner block being prepped into the stepper buffer
static uint8_t pl_prep_index;
static st_data_t *st_prep_data;
static uint8_t st_data_prep_index;
// Returns the index of the next block in the ring buffer
// NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication.
static uint8_t next_block_index(uint8_t block_index)
{
block_index++;
if (block_index == SEGMENT_BUFFER_SIZE) { block_index = 0; }
return(block_index);
}
/* __________________________
/| |\ _________________ ^
/ | | \ /| |\ |
/ | | \ / | | \ s
/ | | | | | \ p
/ | | | | | \ e
+-----+------------------------+---+--+---------------+----+ e
| BLOCK 1 | BLOCK 2 | d
time ----->
The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta
until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after
after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as
+/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND.
*/
// Stepper state initialization. Cycle should only start if the st.cycle_start flag is
// enabled. Startup init and limits call this function but shouldn't start the cycle.
void st_wake_up()
{
// Enable steppers by resetting the stepper disable port
if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) {
STEPPERS_DISABLE_PORT |= (1<<STEPPERS_DISABLE_BIT);
} else {
STEPPERS_DISABLE_PORT &= ~(1<<STEPPERS_DISABLE_BIT);
}
if (sys.state == STATE_CYCLE) {
// Initialize stepper output bits
st.out_bits = settings.invert_mask;
// Initialize step pulse timing from settings.
st.step_pulse_time = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3);
// Enable stepper driver interrupt
st.execute_step = false;
TCNT2 = 0; // Clear Timer2
TIMSK2 |= (1<<OCIE2A); // Enable Timer2 Compare Match A interrupt
TCCR2B = (1<<CS21); // Begin Timer2. Full speed, 1/8 prescaler
}
}
// Stepper shutdown
void st_go_idle()
{
// Disable stepper driver interrupt. Allow Timer0 to finish. It will disable itself.
TIMSK2 &= ~(1<<OCIE2A); // Disable Timer2 interrupt
TCCR2B = 0; // Disable Timer2
busy = false;
// Disable steppers only upon system alarm activated or by user setting to not be kept enabled.
if ((settings.stepper_idle_lock_time != 0xff) || bit_istrue(sys.execute,EXEC_ALARM)) {
// Force stepper dwell to lock axes for a defined amount of time to ensure the axes come to a complete
// stop and not drift from residual inertial forces at the end of the last movement.
delay_ms(settings.stepper_idle_lock_time);
if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) {
STEPPERS_DISABLE_PORT &= ~(1<<STEPPERS_DISABLE_BIT);
} else {
STEPPERS_DISABLE_PORT |= (1<<STEPPERS_DISABLE_BIT);
}
}
}
/* "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Grbl. It is based
on the Pramod Ranade inverse time stepper algorithm, where a timer ticks at a constant
frequency and uses time-distance counters to track when its the approximate time for any
step event. However, the Ranade algorithm, as described, is susceptible to numerical round-off,
meaning that some axes steps may not execute for a given multi-axis motion.
Grbl's algorithm slightly differs by using a single Ranade time-distance counter to manage
a Bresenham line algorithm for multi-axis step events which ensures the number of steps for
each axis are executed exactly. In other words, it uses a Bresenham within a Bresenham algorithm,
where one tracks time(Ranade) and the other steps.
This interrupt pops blocks from the block_buffer and executes them by pulsing the stepper pins
appropriately. It is supported by The Stepper Port Reset Interrupt which it uses to reset the
stepper port after each pulse. The bresenham line tracer algorithm controls all three stepper
outputs simultaneously with these two interrupts.
*/
// NOTE: Average time in this ISR is: 5 usec iterating timers only, 20-25 usec with step event, or
// 15 usec when popping a block. So, ensure Ranade frequency and step pulse times work with this.
ISR(TIMER2_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
// Pulse stepper port pins, if flagged. New block dir will always be set one timer tick
// before any step pulse due to algorithm design.
if (st.execute_step) {
st.execute_step = false;
STEPPING_PORT = ( STEPPING_PORT & ~(DIRECTION_MASK | STEP_MASK) ) | st.out_bits;
TCNT0 = st.step_pulse_time; // Reload Timer0 counter.
TCCR0B = (1<<CS21); // Begin Timer0. Full speed, 1/8 prescaler
}
busy = true;
sei(); // Re-enable interrupts to allow Stepper Port Reset Interrupt to fire on-time.
// NOTE: The remaining code in this ISR will finish before returning to main program.
// If there is no step segment, attempt to pop one from the stepper buffer
if (st.load_flag != LOAD_NOOP) {
// Anything in the buffer? If so, load and initialize next step segment.
if (segment_buffer_head != segment_buffer_tail) {
// NOTE: Loads after a step event. At high rates above 1/2 ISR frequency, there is
// a small chance that this will load at the same time as a step event. Hopefully,
// the overhead for this loading event isn't too much.. possibly 2-5 usec.
// NOTE: The stepper algorithm must control the planner buffer tail as it completes
// the block moves. Otherwise, a feed hold can leave a few step buffer line moves
// without the correct planner block information.
st_current_segment = &segment_buffer[segment_buffer_tail];
// Load number of steps to execute from stepper buffer
st.segment_steps_remaining = st_current_segment->n_step;
st.delta_d = st_current_segment->rate;
// Check if the counters need to be reset for a new planner block
if (st.load_flag == LOAD_BLOCK) {
pl_current_block = plan_get_current_block(); // Should always be there. Stepper buffer handles this.
st_current_data = &segment_data[st_current_segment->st_data_index];
// Initialize direction bits for block
st.out_bits = pl_current_block->direction_bits ^ settings.invert_mask;
st.execute_step = true; // Set flag to set direction bits upon next ISR tick.
// Initialize Bresenham line counters
st.counter_x = (pl_current_block->step_event_count >> 1);
st.counter_y = st.counter_x;
st.counter_z = st.counter_x;
// Initialize inverse time and step rate counter data
st.counter_d = st_current_data->d_next; // d_next always greater than delta_d.
}
st.load_flag = LOAD_NOOP; // Motion loaded. Set no-operation flag until complete.
} else {
// Can't discard planner block here if a feed hold stops in middle of block.
st_go_idle();
bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end
return; // Nothing to do but exit.
}
}
// Iterate inverse time counter. Triggers each Bresenham step event.
st.counter_d -= st.delta_d;
// Execute Bresenham step event, when it's time to do so.
if (st.counter_d < 0) {
st.counter_d += st_current_data->d_next; // Reload inverse time counter
st.out_bits = pl_current_block->direction_bits; // Reset out_bits and reload direction bits
st.execute_step = true;
// Execute step displacement profile by Bresenham line algorithm
st.counter_x -= pl_current_block->steps[X_AXIS];
if (st.counter_x < 0) {
st.out_bits |= (1<<X_STEP_BIT);
st.counter_x += pl_current_block->step_event_count;
if (st.out_bits & (1<<X_DIRECTION_BIT)) { sys.position[X_AXIS]--; }
else { sys.position[X_AXIS]++; }
}
st.counter_y -= pl_current_block->steps[Y_AXIS];
if (st.counter_y < 0) {
st.out_bits |= (1<<Y_STEP_BIT);
st.counter_y += pl_current_block->step_event_count;
if (st.out_bits & (1<<Y_DIRECTION_BIT)) { sys.position[Y_AXIS]--; }
else { sys.position[Y_AXIS]++; }
}
st.counter_z -= pl_current_block->steps[Z_AXIS];
if (st.counter_z < 0) {
st.out_bits |= (1<<Z_STEP_BIT);
st.counter_z += pl_current_block->step_event_count;
if (st.out_bits & (1<<Z_DIRECTION_BIT)) { sys.position[Z_AXIS]--; }
else { sys.position[Z_AXIS]++; }
}
// Check step events for trapezoid change or end of block.
st.segment_steps_remaining--; // Decrement step events count
if (st.segment_steps_remaining == 0) {
// Line move is complete, set load line flag to check for new move.
// Check if last line move in planner block. Discard if so.
if (st_current_segment->flag == ST_END_OF_BLOCK) {
plan_discard_current_block();
st.load_flag = LOAD_BLOCK;
} else {
st.load_flag = LOAD_LINE;
}
// Discard current block
if (segment_buffer_head != segment_buffer_tail) {
segment_buffer_tail = next_block_index( segment_buffer_tail );
}
// NOTE: sys.position updates could be done here. The bresenham counters can have
// their own fast 8-bit addition-only counters. Here we would check the direction and
// apply it to sys.position accordingly. However, this could take too much time.
}
st.out_bits ^= settings.invert_mask; // Apply step port invert mask
}
busy = false;
// SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT;
}
// The Stepper Port Reset Interrupt: Timer0 OVF interrupt handles the falling edge of the
// step pulse. This should always trigger before the next Timer2 COMPA interrupt and independently
// finish, if Timer2 is disabled after completing a move.
ISR(TIMER0_OVF_vect)
{
STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | (settings.invert_mask & STEP_MASK);
TCCR0B = 0; // Disable timer until needed.
}
// Reset and clear stepper subsystem variables
void st_reset()
{
memset(&st, 0, sizeof(st));
pl_current_block = NULL;
pl_prep_block = NULL;
pl_prep_index = 0;
st_data_prep_index = 0;
st.load_flag = LOAD_BLOCK;
busy = false;
segment_buffer_tail = 0;
segment_next_head = 1;
}
// Initialize and start the stepper motor subsystem
void st_init()
{
// Configure directions of interface pins
STEPPING_DDR |= STEPPING_MASK;
STEPPING_PORT = (STEPPING_PORT & ~STEPPING_MASK) | settings.invert_mask;
STEPPERS_DISABLE_DDR |= 1<<STEPPERS_DISABLE_BIT;
// Configure Timer 2
TIMSK2 &= ~(1<<OCIE2A); // Disable Timer2 interrupt while configuring it
TCCR2B = 0; // Disable Timer2 until needed
TCNT2 = 0; // Clear Timer2 counter
TCCR2A = (1<<WGM21); // Set CTC mode
OCR2A = (F_CPU/ISR_TICKS_PER_SECOND)/8 - 1; // Set Timer2 CTC rate
// Configure Timer 0
TIMSK0 &= ~(1<<TOIE0);
TCCR0A = 0; // Normal operation
TCCR0B = 0; // Disable Timer0 until needed
TIMSK0 |= (1<<TOIE0); // Enable overflow interrupt
// Start in the idle state, but first wake up to check for keep steppers enabled option.
st_wake_up();
st_go_idle();
}
// Planner external interface to start stepper interrupt and execute the blocks in queue. Called
// by the main program functions: planner auto-start and run-time command execution.
void st_cycle_start()
{
if (sys.state == STATE_QUEUED) {
sys.state = STATE_CYCLE;
st_wake_up();
}
}
// Execute a feed hold with deceleration, only during cycle. Called by main program.
void st_feed_hold()
{
if (sys.state == STATE_CYCLE) {
sys.state = STATE_HOLD;
sys.auto_start = false; // Disable planner auto start upon feed hold.
}
}
// Reinitializes the cycle plan and stepper system after a feed hold for a resume. Called by
// runtime command execution in the main program, ensuring that the planner re-plans safely.
// NOTE: Bresenham algorithm variables are still maintained through both the planner and stepper
// cycle reinitializations. The stepper path should continue exactly as if nothing has happened.
// Only the planner de/ac-celerations profiles and stepper rates have been updated.
void st_cycle_reinitialize()
{
if (pl_current_block != NULL) {
// Replan buffer from the feed hold stop location.
// TODO: Need to add up all of the step events in the current planner block to give
// back to the planner. Should only need it for the current block.
// BUT! The planner block millimeters is all changed and may be changed into the next
// planner block. The block millimeters would need to be recalculated via step counts
// and the mm/step variable.
// OR. Do we plan the feed hold itself down with the planner.
plan_cycle_reinitialize(st_current_data->step_events_remaining);
st.ramp_type = RAMP_ACCEL;
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2;
st.delta_d = 0;
sys.state = STATE_QUEUED;
} else {
sys.state = STATE_IDLE;
}
}
/* Preps stepper buffer. Called from main program.
NOTE: There doesn't seem to be a great way to figure out how many steps occur within
a set number of ISR ticks. Numerical round-off and CPU overhead always seems to be a
critical problem. So, either numerical round-off checks could be made to account for
them, while CPU overhead could be minimized in some way, or we can flip the algorithm
around to have the stepper algorithm track number of steps over an indeterminant amount
of time instead.
In other words, we use the planner velocity floating point data to get an estimate of
the number of steps we want to execute. We then back out the approximate velocity for
the planner to use, which should be much more robust to round-off error. The main problem
now is that we are loading the stepper algorithm to handle acceleration now, rather than
pre-calculating with the main program. This approach does make sense in the way that
planner velocities and stepper profiles can be traced more accurately.
Which is better? Very hard to tell. The time-based algorithm would be able to handle
Bresenham step adaptive-resolution much easier and cleaner. Whereas, the step-based would
require some additional math in the stepper algorithm to adjust on the fly, plus adaptation
would occur in a non-deterministic manner.
I suppose it wouldn't hurt to build both to see what's better. Just a lot more work.
TODO: Need to describe the importance of continuations of step pulses between ramp states
and planner blocks. This has to do with Alden's problem with step "phase". The things I've
been doing here limit this phase issue by truncating some of the ramp timing for certain
events like deceleration initialization and end of block.
*/
void st_prep_buffer()
{
while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer.
// Determine if we need to load a new planner block.
if (pl_prep_block == NULL) {
pl_prep_block = plan_get_block_by_index(pl_prep_index);
if (pl_prep_block == NULL) { return; } // No more planner blocks. Let stepper finish out.
// Prepare commonly shared planner block data for the ensuing step buffer moves
st_data_prep_index = next_block_index(st_data_prep_index);
st_prep_data = &segment_data[st_data_prep_index];
// Initialize Bresenham variables
st_prep_data->step_events_remaining = pl_prep_block->step_event_count;
// Convert new block to stepper variables.
// NOTE: This data can change mid-block from normal planner updates and feedrate overrides. Must
// be maintained as these execute.
// TODO: If the planner updates this block, particularly from a deceleration to an acceleration,
// we must reload the initial rate data, such that the velocity profile is re-constructed correctly.
// st_prep_data->initial_rate = ceil(sqrt(pl_prep_block->entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
// st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
// This data doesn't change. Could be performed in the planner, but fits nicely here.
// Although, acceleration can change for S-curves. So keep it here.
// st_prep_data->rate_delta = ceil(pl_prep_block->acceleration*
// ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic)
// This definitely doesn't change, but could be precalculated in a way to help some of the
// math in this handler, i.e. millimeters per step event data.
st_prep_data->d_next = ceil((pl_prep_block->millimeters*INV_TIME_MULTIPLIER)/pl_prep_block->step_event_count); // (mult*mm/step)
st_prep_data->mm_per_step = pl_prep_block->millimeters/pl_prep_block->step_event_count;
}
/*
// Check if planner has changed block exit parameters. If so, then we have to update the
// velocity profile for the remainder of this block. Otherwise, the block hasn't changed.
if (exit_speed_sqr != last_exit_speed_sqr) {
intersect_distance = 0.5*( millimeters + (entry_speed_sqr-exit_speed_sqr)/(2*acceleration) );
if (intersect_distance <= 0) { // Deceleration only.
final_rate_sqr = initial_rate_sqr - 2*acceleration*segment_distance;
block->decelerate_after = 0;
} else {
decelerate_after = (block->nominal_speed_sqr - exit_speed_sqr)/(2*block->acceleration);
if (decelerate_after > intersect_distance) { decelerate_after = intersect_distance; }
if (decelerate_after > block->millimeters) { decelerate_after = block->millimeters; }
}
}
n_step = 100; // Estimate distance we're going to travel in this segment
if (n_step > step_events_remaining) { n_step = step_events_remaining; };
segment_distance = n_step*mm_per_step; // True distance traveled
// ISSUE: Either weighted average speed or truncated segments must be used. Otherwise
// accelerations, in particular high value, will not perform correctly.
if (distance_traveled < decelerate_after) {
if (segment_distance + distance_traveled > decelerate_after) {
n_step = ceil((decelerate_after-distance_traveled)/mm_per_step);
segment_distance = n_step*mm_per_step;
}
}
// based on time?
time = incremental time;
v_exit = v_entry + acceleration*time;
if (v_exit > v_nominal) {
time_accel = (v_nominal-v_entry)/acceleration;
distance = v_entry*time_accel + 0.5*acceleration*time_accel**2;
time_remaining = time - time_accel;
}
distance = v_entry*time + 0.5*acceleration*time*time;
t_accel = (v_nominal - v_entry) / acceleration;
t_decel = (v_exit - v_nominal) / -acceleration;
dt = (v_entry-v_exit)/acceleration;
if
// What's the speed of this segment? Computing per segment can get expensive, but this
// would allow S-curves to be installed pretty easily here.
if (initial_speed < nominal_speed) {
if (initial_distance < decelerate_after) { // Acceleration
exit_speed = sqrt(initial_speed*initial_speed + 2*acceleration*distance);
if (exit_speed > nominal_speed) { exit_speed = nominal_speed; }
} else { // Deceleration
exit_speed = sqrt(initial_speed*initial_speed - 2*acceleration*distance);
}
average_speed = 0.5*(initial_speed + exit_speed);
} else { // Cruise
average_speed = nominal_speed;
}
segment_rate = ceil(average_speed*(INV_TIME_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
if (segment_rate < MINIMUM_STEP_RATE) { segment_rate = MINIMUM_STEP_RATE; }
*/
/*
TODO: Need to check for a planner flag to indicate a change to this planner block.
If so, need to check for a change in acceleration state, from deceleration to acceleration,
to reset the stepper ramp counters and the initial_rate data to trace the new
ac/de-celeration profile correctly.
No change conditions:
- From nominal speed to acceleration from feedrate override
- From nominal speed to new deceleration.
- From acceleration to new deceleration point later or cruising point.
- From acceleration to immediate deceleration? Can happen during feedrate override
and slowing down, but likely ok by enforcing the normal ramp counter protocol.
Change conditions:
- From deceleration to acceleration, i.e. common with jogging when new blocks are added.
*/
st_segment_t *st_prep_block = &segment_buffer[segment_buffer_head];
st_prep_block->st_data_index = st_data_prep_index;
// TODO: How do you cheaply compute n_step without a sqrt()? Could be performed as 'bins'.
st_prep_block->n_step = 100; //floor( (exit_speed*approx_time)/mm_per_step );
// st_segment->n_step = max(st_segment->n_step,MINIMUM_STEPS_PER_BLOCK); // Ensure it moves for very slow motions?
// st_segment->n_step = min(st_segment->n_step,MAXIMUM_STEPS_PER_BLOCK); // Prevent unsigned int8 overflow.
// Check if n_step exceeds steps remaining in planner block. If so, truncate.
if (st_prep_block->n_step > st_prep_data->step_events_remaining) {
st_prep_block->n_step = st_prep_data->step_events_remaining;
}
// Check if n_step exceeds decelerate point in block. Need to perform this so that the
// ramp counters are reset correctly in the stepper algorithm. Can be 1 step, but should
// be OK since it is likely moving at a fast rate already.
if (st_prep_block->n_step > pl_prep_block->decelerate_after) {
st_prep_block->n_step = pl_prep_block->decelerate_after;
}
float distance, exit_speed_sqr;
distance = st_prep_block->n_step*st_prep_data->mm_per_step; // Always greater than zero
if (st_prep_data->step_events_remaining >= pl_prep_block->decelerate_after) {
exit_speed_sqr = pl_prep_block->entry_speed_sqr - 2*pl_prep_block->acceleration*distance;
// Set ISR tick reset flag for deceleration ramp.
} else { // Acceleration or cruising ramp
if (pl_prep_block->entry_speed_sqr < pl_prep_block->nominal_speed_sqr) {
exit_speed_sqr = pl_prep_block->entry_speed_sqr + 2*pl_prep_block->acceleration*distance;
if (exit_speed_sqr > pl_prep_block->nominal_speed_sqr) { exit_speed_sqr = pl_prep_block->nominal_speed_sqr; }
} else {
exit_speed_sqr = pl_prep_block->nominal_speed_sqr;
}
}
// Adjust inverse time counter for ac/de-celerations
if (st.ramp_type) {
st.ramp_count--; // Tick acceleration ramp counter
if (st.ramp_count == 0) { // Adjust step rate when its time
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter
if (st.ramp_type == RAMP_ACCEL) { // Adjust velocity for acceleration
st.delta_d += st_current_data->rate_delta;
if (st.delta_d >= st_current_data->nominal_rate) { // Reached nominal rate.
st.delta_d = st_current_data->nominal_rate; // Set cruising velocity
st.ramp_type = RAMP_NOOP_CRUISE; // Set ramp flag to ignore
}
} else { // Adjust velocity for deceleration
if (st.delta_d > st_current_data->rate_delta) {
st.delta_d -= st_current_data->rate_delta;
} else {
// Moving near zero feed rate. Gracefully slow down.
st.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow.
// Check for and handle feed hold exit? At this point, machine is stopped.
}
}
// Finalize adjusted step rate. Ensure minimum.
if (st.delta_d < MINIMUM_STEP_RATE) { st.d_per_tick = MINIMUM_STEP_RATE; }
else { st.d_per_tick = st.delta_d; }
}
}
// During feed hold, do not update rate or ramp type. Keep decelerating.
if (sys.state == STATE_CYCLE) {
st.delta_d = st_current_data->initial_rate;
if (st.delta_d == st_current_data->nominal_rate) {
ramp_type = RAMP_NOOP_CRUISE;
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid
}
// Acceleration and cruise handled by ramping. Just check for deceleration.
if (st_current_segment->flag == ST_NOOP) {
if (st.ramp_type == RAMP_NOOP_CRUISE) {
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid
} else {
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; // Set ramp counter for triangle
}
st.ramp_type = RAMP_DECEL;
}
// Update planner block variables.
pl_prep_block->entry_speed_sqr = max(0.0,exit_speed_sqr);
// pl_prep_block->max_entry_speed_sqr = exit_speed_sqr; // ??? Overwrites the corner speed. May need separate variable.
pl_prep_block->millimeters -= distance; // Potential round-off error near end of block.
pl_prep_block->millimeters = max(0.0,pl_prep_block->millimeters); // Shouldn't matter.
// Update stepper block variables.
st_prep_data->step_events_remaining -= st_prep_block->n_step;
if ( st_prep_data->step_events_remaining == 0 ) {
// Move planner pointer to next block
st_prep_block->flag = ST_END_OF_BLOCK;
pl_prep_index = next_block_index(pl_prep_index);
pl_prep_block = NULL;
} else {
st_prep_block->flag = ST_NOOP;
}
// New step block completed. Increment step buffer indices.
segment_buffer_head = segment_next_head;
segment_next_head = next_block_index(segment_buffer_head);
}
}

387
stepper_old.c Normal file
View File

@ -0,0 +1,387 @@
/*
stepper.c - stepper motor driver: executes motion plans using stepper motors
Part of Grbl
Copyright (c) 2011-2013 Sungeun K. Jeon
Copyright (c) 2009-2011 Simen Svale Skogsrud
Grbl is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
Grbl is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with Grbl. If not, see <http://www.gnu.org/licenses/>.
*/
/* The timer calculations of this module informed by the 'RepRap cartesian firmware' by Zack Smith
and Philipp Tiefenbacher. */
#include <avr/interrupt.h>
#include "stepper.h"
#include "config.h"
#include "settings.h"
#include "planner.h"
// Some useful constants
#define TICKS_PER_MICROSECOND (F_CPU/1000000)
#define CRUISE_RAMP 0
#define ACCEL_RAMP 1
#define DECEL_RAMP 2
// Stepper state variable. Contains running data and trapezoid variables.
typedef struct {
// Used by the bresenham line algorithm
int32_t counter_x, // Counter variables for the bresenham line tracer
counter_y,
counter_z;
int32_t event_count; // Total event count. Retained for feed holds.
int32_t step_events_remaining; // Steps remaining in motion
// Used by Pramod Ranade inverse time algorithm
int32_t delta_d; // Ranade distance traveled per interrupt tick
int32_t d_counter; // Ranade distance traveled since last step event
uint8_t ramp_count; // Acceleration interrupt tick counter.
uint8_t ramp_type; // Ramp type variable.
uint8_t execute_step; // Flags step execution for each interrupt.
} stepper_t;
static stepper_t st;
static block_t *current_block; // A pointer to the block currently being traced
// Used by the stepper driver interrupt
static uint8_t step_pulse_time; // Step pulse reset time after step rise
static uint8_t out_bits; // The next stepping-bits to be output
// NOTE: If the main interrupt is guaranteed to be complete before the next interrupt, then
// this blocking variable is no longer needed. Only here for safety reasons.
static volatile uint8_t busy; // True when "Stepper Driver Interrupt" is being serviced. Used to avoid retriggering that handler.
// __________________________
// /| |\ _________________ ^
// / | | \ /| |\ |
// / | | \ / | | \ s
// / | | | | | \ p
// / | | | | | \ e
// +-----+------------------------+---+--+---------------+----+ e
// | BLOCK 1 | BLOCK 2 | d
//
// time ----->
//
// The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta
// until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after
// after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as
// +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND.
// Stepper state initialization. Cycle should only start if the st.cycle_start flag is
// enabled. Startup init and limits call this function but shouldn't start the cycle.
void st_wake_up()
{
// Enable steppers by resetting the stepper disable port
if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) {
STEPPERS_DISABLE_PORT |= (1<<STEPPERS_DISABLE_BIT);
} else {
STEPPERS_DISABLE_PORT &= ~(1<<STEPPERS_DISABLE_BIT);
}
if (sys.state == STATE_CYCLE) {
// Initialize stepper output bits
out_bits = settings.invert_mask;
// Initialize step pulse timing from settings.
step_pulse_time = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3);
// Enable stepper driver interrupt
st.execute_step = false;
TCNT2 = 0; // Clear Timer2
TIMSK2 |= (1<<OCIE2A); // Enable Timer2 Compare Match A interrupt
TCCR2B = (1<<CS21); // Begin Timer2. Full speed, 1/8 prescaler
}
}
// Stepper shutdown
void st_go_idle()
{
// Disable stepper driver interrupt. Allow Timer0 to finish. It will disable itself.
TIMSK2 &= ~(1<<OCIE2A); // Disable Timer2 interrupt
TCCR2B = 0; // Disable Timer2
busy = false;
// Disable steppers only upon system alarm activated or by user setting to not be kept enabled.
if ((settings.stepper_idle_lock_time != 0xff) || bit_istrue(sys.execute,EXEC_ALARM)) {
// Force stepper dwell to lock axes for a defined amount of time to ensure the axes come to a complete
// stop and not drift from residual inertial forces at the end of the last movement.
delay_ms(settings.stepper_idle_lock_time);
if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) {
STEPPERS_DISABLE_PORT &= ~(1<<STEPPERS_DISABLE_BIT);
} else {
STEPPERS_DISABLE_PORT |= (1<<STEPPERS_DISABLE_BIT);
}
}
}
// "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Grbl. It is based
// on the Pramod Ranade inverse time stepper algorithm, where a timer ticks at a constant
// frequency and uses time-distance counters to track when its the approximate time for any
// step event. However, the Ranade algorithm, as described, is susceptible to numerical round-off,
// meaning that some axes steps may not execute for a given multi-axis motion.
// Grbl's algorithm slightly differs by using a single Ranade time-distance counter to manage
// a Bresenham line algorithm for multi-axis step events which ensures the number of steps for
// each axis are executed exactly. In other words, it uses a Bresenham within a Bresenham algorithm,
// where one tracks time(Ranade) and the other steps.
// This interrupt pops blocks from the block_buffer and executes them by pulsing the stepper pins
// appropriately. It is supported by The Stepper Port Reset Interrupt which it uses to reset the
// stepper port after each pulse. The bresenham line tracer algorithm controls all three stepper
// outputs simultaneously with these two interrupts.
//
// NOTE: Average time in this ISR is: 5 usec iterating timers only, 20-25 usec with step event, or
// 15 usec when popping a block. So, ensure Ranade frequency and step pulse times work with this.
ISR(TIMER2_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
// Pulse stepper port pins, if flagged. New block dir will always be set one timer tick
// before any step pulse due to algorithm design.
if (st.execute_step) {
st.execute_step = false;
STEPPING_PORT = ( STEPPING_PORT & ~(DIRECTION_MASK | STEP_MASK) ) | out_bits;
TCNT0 = step_pulse_time; // Reload Timer0 counter.
TCCR0B = (1<<CS21); // Begin Timer0. Full speed, 1/8 prescaler
}
busy = true;
sei(); // Re-enable interrupts. This ISR will still finish before returning to main program.
// If there is no current block, attempt to pop one from the buffer
if (current_block == NULL) {
// Anything in the buffer? If so, initialize next motion.
current_block = plan_get_current_block();
if (current_block != NULL) {
// By algorithm design, the loading of the next block never coincides with a step event,
// since there is always one Ranade timer tick before a step event occurs. This means
// that the Bresenham counter math never is performed at the same time as the loading
// of a block, hence helping minimize total time spent in this interrupt.
// Initialize direction bits for block
out_bits = current_block->direction_bits ^ settings.invert_mask;
st.execute_step = true; // Set flag to set direction bits.
// Initialize Bresenham variables
st.counter_x = (current_block->step_event_count >> 1);
st.counter_y = st.counter_x;
st.counter_z = st.counter_x;
st.event_count = current_block->step_event_count;
st.step_events_remaining = st.event_count;
// During feed hold, do not update Ranade counter, rate, or ramp type. Keep decelerating.
if (sys.state == STATE_CYCLE) {
// Initialize Ranade variables
st.d_counter = current_block->d_next;
st.delta_d = current_block->initial_rate;
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2;
// Initialize ramp type.
if (st.step_events_remaining == current_block->decelerate_after) { st.ramp_type = DECEL_RAMP; }
else if (st.delta_d == current_block->nominal_rate) { st.ramp_type = CRUISE_RAMP; }
else { st.ramp_type = ACCEL_RAMP; }
}
} else {
st_go_idle();
bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end
return; // Nothing to do but exit.
}
}
// Adjust inverse time counter for ac/de-celerations
if (st.ramp_type) {
// Tick acceleration ramp counter
st.ramp_count--;
if (st.ramp_count == 0) {
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter
if (st.ramp_type == ACCEL_RAMP) { // Adjust velocity for acceleration
st.delta_d += current_block->rate_delta;
if (st.delta_d >= current_block->nominal_rate) { // Reached cruise state.
st.ramp_type = CRUISE_RAMP;
st.delta_d = current_block->nominal_rate; // Set cruise velocity
}
} else if (st.ramp_type == DECEL_RAMP) { // Adjust velocity for deceleration
if (st.delta_d > current_block->rate_delta) {
st.delta_d -= current_block->rate_delta;
} else {
st.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow.
}
}
}
}
// Iterate Pramod Ranade inverse time counter. Triggers each Bresenham step event.
if (st.delta_d < MINIMUM_STEP_RATE) { st.d_counter -= MINIMUM_STEP_RATE; }
else { st.d_counter -= st.delta_d; }
// Execute Bresenham step event, when it's time to do so.
if (st.d_counter < 0) {
st.d_counter += current_block->d_next;
// Check for feed hold state and execute accordingly.
if (sys.state == STATE_HOLD) {
if (st.ramp_type != DECEL_RAMP) {
st.ramp_type = DECEL_RAMP;
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2;
}
if (st.delta_d <= current_block->rate_delta) {
st_go_idle();
bit_true(sys.execute,EXEC_CYCLE_STOP);
return;
}
}
// TODO: Vary Bresenham resolution for smoother motions or enable faster step rates (>20kHz).
out_bits = current_block->direction_bits; // Reset out_bits and reload direction bits
st.execute_step = true;
// Execute step displacement profile by Bresenham line algorithm
st.counter_x -= current_block->steps[X_AXIS];
if (st.counter_x < 0) {
out_bits |= (1<<X_STEP_BIT);
st.counter_x += st.event_count;
if (out_bits & (1<<X_DIRECTION_BIT)) { sys.position[X_AXIS]--; }
else { sys.position[X_AXIS]++; }
}
st.counter_y -= current_block->steps[Y_AXIS];
if (st.counter_y < 0) {
out_bits |= (1<<Y_STEP_BIT);
st.counter_y += st.event_count;
if (out_bits & (1<<Y_DIRECTION_BIT)) { sys.position[Y_AXIS]--; }
else { sys.position[Y_AXIS]++; }
}
st.counter_z -= current_block->steps[Z_AXIS];
if (st.counter_z < 0) {
out_bits |= (1<<Z_STEP_BIT);
st.counter_z += st.event_count;
if (out_bits & (1<<Z_DIRECTION_BIT)) { sys.position[Z_AXIS]--; }
else { sys.position[Z_AXIS]++; }
}
// Check step events for trapezoid change or end of block.
st.step_events_remaining--; // Decrement step events count
if (st.step_events_remaining) {
if (st.ramp_type != DECEL_RAMP) {
// Acceleration and cruise handled by ramping. Just check for deceleration.
if (st.step_events_remaining <= current_block->decelerate_after) {
st.ramp_type = DECEL_RAMP;
if (st.step_events_remaining == current_block->decelerate_after) {
if (st.delta_d == current_block->nominal_rate) {
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid
} else {
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; // Set ramp counter for triangle
}
}
}
}
} else {
// If current block is finished, reset pointer
current_block = NULL;
plan_discard_current_block();
}
out_bits ^= settings.invert_mask; // Apply step port invert mask
}
busy = false;
// SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT;
}
// The Stepper Port Reset Interrupt: Timer0 OVF interrupt handles the falling edge of the
// step pulse. This should always trigger before the next Timer2 COMPA interrupt and independently
// finish, if Timer2 is disabled after completing a move.
ISR(TIMER0_OVF_vect)
{
STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | (settings.invert_mask & STEP_MASK);
TCCR0B = 0; // Disable timer until needed.
}
// Reset and clear stepper subsystem variables
void st_reset()
{
memset(&st, 0, sizeof(st));
current_block = NULL;
busy = false;
}
// Initialize and start the stepper motor subsystem
void st_init()
{
// Configure directions of interface pins
STEPPING_DDR |= STEPPING_MASK;
STEPPING_PORT = (STEPPING_PORT & ~STEPPING_MASK) | settings.invert_mask;
STEPPERS_DISABLE_DDR |= 1<<STEPPERS_DISABLE_BIT;
// Configure Timer 2
TIMSK2 &= ~(1<<OCIE2A); // Disable Timer2 interrupt while configuring it
TCCR2B = 0; // Disable Timer2 until needed
TCNT2 = 0; // Clear Timer2 counter
TCCR2A = (1<<WGM21); // Set CTC mode
OCR2A = (F_CPU/ISR_TICKS_PER_SECOND)/8 - 1; // Set Timer2 CTC rate
// Configure Timer 0
TIMSK0 &= ~(1<<TOIE0);
TCCR0A = 0; // Normal operation
TCCR0B = 0; // Disable Timer0 until needed
TIMSK0 |= (1<<TOIE0); // Enable overflow interrupt
// Start in the idle state, but first wake up to check for keep steppers enabled option.
st_wake_up();
st_go_idle();
}
// Planner external interface to start stepper interrupt and execute the blocks in queue. Called
// by the main program functions: planner auto-start and run-time command execution.
void st_cycle_start()
{
if (sys.state == STATE_QUEUED) {
sys.state = STATE_CYCLE;
st_wake_up();
}
}
// Execute a feed hold with deceleration, only during cycle. Called by main program.
void st_feed_hold()
{
if (sys.state == STATE_CYCLE) {
sys.state = STATE_HOLD;
sys.auto_start = false; // Disable planner auto start upon feed hold.
}
}
// Reinitializes the cycle plan and stepper system after a feed hold for a resume. Called by
// runtime command execution in the main program, ensuring that the planner re-plans safely.
// NOTE: Bresenham algorithm variables are still maintained through both the planner and stepper
// cycle reinitializations. The stepper path should continue exactly as if nothing has happened.
// Only the planner de/ac-celerations profiles and stepper rates have been updated.
void st_cycle_reinitialize()
{
if (current_block != NULL) {
// Replan buffer from the feed hold stop location.
plan_cycle_reinitialize(st.step_events_remaining);
st.ramp_type = ACCEL_RAMP;
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2;
st.delta_d = 0;
sys.state = STATE_QUEUED;
} else {
sys.state = STATE_IDLE;
}
}