Refactor and optimize Stepper/Planner

Better encapsulation and considerably reduce stepper jitter
This commit is contained in:
etagle 2018-05-09 02:17:53 -03:00 committed by Scott Lahteine
parent 0566badcef
commit a11eb50a3e
16 changed files with 975 additions and 738 deletions

View file

@ -269,7 +269,7 @@ bool pin_is_protected(const pin_t pin) {
} }
void quickstop_stepper() { void quickstop_stepper() {
stepper.quick_stop(); planner.quick_stop();
planner.synchronize(); planner.synchronize();
set_current_from_steppers_for_axis(ALL_AXES); set_current_from_steppers_for_axis(ALL_AXES);
SYNC_PLAN_POSITION_KINEMATIC(); SYNC_PLAN_POSITION_KINEMATIC();

View file

@ -262,7 +262,8 @@
z_position = end[Z_AXIS]; z_position = end[Z_AXIS];
} }
planner.buffer_segment(rx, ry, z_position + z0, e_position, feed_rate, extruder); if (!planner.buffer_segment(rx, ry, z_position + z0, e_position, feed_rate, extruder))
break;
} //else printf("FIRST MOVE PRUNED "); } //else printf("FIRST MOVE PRUNED ");
} }
@ -319,7 +320,8 @@
e_position = end[E_AXIS]; e_position = end[E_AXIS];
z_position = end[Z_AXIS]; z_position = end[Z_AXIS];
} }
planner.buffer_segment(rx, next_mesh_line_y, z_position + z0, e_position, feed_rate, extruder); if (!planner.buffer_segment(rx, next_mesh_line_y, z_position + z0, e_position, feed_rate, extruder))
break;
current_yi += dyi; current_yi += dyi;
yi_cnt--; yi_cnt--;
} }
@ -342,7 +344,8 @@
z_position = end[Z_AXIS]; z_position = end[Z_AXIS];
} }
planner.buffer_segment(next_mesh_line_x, ry, z_position + z0, e_position, feed_rate, extruder); if (!planner.buffer_segment(next_mesh_line_x, ry, z_position + z0, e_position, feed_rate, extruder))
break;
current_xi += dxi; current_xi += dxi;
xi_cnt--; xi_cnt--;
} }

View file

@ -33,7 +33,7 @@
void GcodeSuite::M540() { void GcodeSuite::M540() {
if (parser.seen('S')) if (parser.seen('S'))
stepper.abort_on_endstop_hit = parser.value_bool(); planner.abort_on_endstop_hit = parser.value_bool();
} }

View file

@ -47,7 +47,7 @@ void GcodeSuite::M18_M84() {
else { else {
bool all_axis = !(parser.seen('X') || parser.seen('Y') || parser.seen('Z') || parser.seen('E')); bool all_axis = !(parser.seen('X') || parser.seen('Y') || parser.seen('Z') || parser.seen('E'));
if (all_axis) { if (all_axis) {
stepper.finish_and_disable(); planner.finish_and_disable();
} }
else { else {
planner.synchronize(); planner.synchronize();

View file

@ -95,7 +95,7 @@
*/ */
void GcodeSuite::M81() { void GcodeSuite::M81() {
thermalManager.disable_all_heaters(); thermalManager.disable_all_heaters();
stepper.finish_and_disable(); planner.finish_and_disable();
#if FAN_COUNT > 0 #if FAN_COUNT > 0
for (uint8_t i = 0; i < FAN_COUNT; i++) fanSpeeds[i] = 0; for (uint8_t i = 0; i < FAN_COUNT; i++) fanSpeeds[i] = 0;

View file

@ -197,14 +197,17 @@ void plan_arc(
// i.e., Complete the angular vector in the given time. // i.e., Complete the angular vector in the given time.
inverse_kinematics(raw); inverse_kinematics(raw);
ADJUST_DELTA(raw); ADJUST_DELTA(raw);
planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], raw[Z_AXIS], raw[E_AXIS], HYPOT(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB) * inverse_secs, active_extruder); if (!planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], raw[Z_AXIS], raw[E_AXIS], HYPOT(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB) * inverse_secs, active_extruder))
break;
oldA = delta[A_AXIS]; oldB = delta[B_AXIS]; oldA = delta[A_AXIS]; oldB = delta[B_AXIS];
#elif HAS_UBL_AND_CURVES #elif HAS_UBL_AND_CURVES
float pos[XYZ] = { raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS] }; float pos[XYZ] = { raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS] };
planner.apply_leveling(pos); planner.apply_leveling(pos);
planner.buffer_segment(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], raw[E_AXIS], fr_mm_s, active_extruder); if (!planner.buffer_segment(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], raw[E_AXIS], fr_mm_s, active_extruder))
break;
#else #else
planner.buffer_line_kinematic(raw, fr_mm_s, active_extruder); if (!planner.buffer_line_kinematic(raw, fr_mm_s, active_extruder))
break;
#endif #endif
} }

View file

@ -2421,12 +2421,10 @@ void lcd_quick_feedback(const bool clear_buttons) {
void _lcd_do_nothing() {} void _lcd_do_nothing() {}
void _lcd_hard_stop() { void _lcd_hard_stop() {
stepper.quick_stop();
const screenFunc_t old_screen = currentScreen; const screenFunc_t old_screen = currentScreen;
currentScreen = _lcd_do_nothing; currentScreen = _lcd_do_nothing;
while (planner.movesplanned()) idle(); planner.quick_stop();
currentScreen = old_screen; currentScreen = old_screen;
stepper.cleaning_buffer_counter = 0;
set_current_from_steppers_for_axis(ALL_AXES); set_current_from_steppers_for_axis(ALL_AXES);
sync_plan_position(); sync_plan_position();
} }
@ -3856,7 +3854,7 @@ void lcd_quick_feedback(const bool clear_buttons) {
// M540 S - Abort on endstop hit when SD printing // M540 S - Abort on endstop hit when SD printing
#if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED) #if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
MENU_ITEM_EDIT(bool, MSG_ENDSTOP_ABORT, &stepper.abort_on_endstop_hit); MENU_ITEM_EDIT(bool, MSG_ENDSTOP_ABORT, &planner.abort_on_endstop_hit);
#endif #endif
END_MENU(); END_MENU();

View file

@ -208,7 +208,7 @@ void Endstops::report_state() {
#endif #endif
#define _ENDSTOP_HIT_ECHO(A,C) do{ \ #define _ENDSTOP_HIT_ECHO(A,C) do{ \
SERIAL_ECHOPAIR(" " STRINGIFY(A) ":", stepper.triggered_position_mm(_AXIS(A))); \ SERIAL_ECHOPAIR(" " STRINGIFY(A) ":", planner.triggered_position_mm(_AXIS(A))); \
_SET_STOP_CHAR(A,C); }while(0) _SET_STOP_CHAR(A,C); }while(0)
#define _ENDSTOP_HIT_TEST(A,C) \ #define _ENDSTOP_HIT_TEST(A,C) \
@ -238,7 +238,7 @@ void Endstops::report_state() {
hit_on_purpose(); hit_on_purpose();
#if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED) && ENABLED(SDSUPPORT) #if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED) && ENABLED(SDSUPPORT)
if (stepper.abort_on_endstop_hit) { if (planner.abort_on_endstop_hit) {
card.sdprinting = false; card.sdprinting = false;
card.closefile(); card.closefile();
quickstop_stepper(); quickstop_stepper();
@ -349,7 +349,7 @@ void Endstops::update() {
UPDATE_ENDSTOP_BIT(AXIS, MINMAX); \ UPDATE_ENDSTOP_BIT(AXIS, MINMAX); \
if (TEST_ENDSTOP(_ENDSTOP(AXIS, MINMAX))) { \ if (TEST_ENDSTOP(_ENDSTOP(AXIS, MINMAX))) { \
_ENDSTOP_HIT(AXIS, MINMAX); \ _ENDSTOP_HIT(AXIS, MINMAX); \
stepper.endstop_triggered(_AXIS(AXIS)); \ planner.endstop_triggered(_AXIS(AXIS)); \
} \ } \
}while(0) }while(0)
@ -358,9 +358,9 @@ void Endstops::update() {
if (G38_move) { if (G38_move) {
UPDATE_ENDSTOP_BIT(Z, MIN_PROBE); UPDATE_ENDSTOP_BIT(Z, MIN_PROBE);
if (TEST_ENDSTOP(_ENDSTOP(Z, MIN_PROBE))) { if (TEST_ENDSTOP(_ENDSTOP(Z, MIN_PROBE))) {
if (stepper.current_block->steps[_AXIS(X)] > 0) { _ENDSTOP_HIT(X, MIN); stepper.endstop_triggered(_AXIS(X)); } if (stepper.current_block->steps[_AXIS(X)] > 0) { _ENDSTOP_HIT(X, MIN); planner.endstop_triggered(_AXIS(X)); }
else if (stepper.current_block->steps[_AXIS(Y)] > 0) { _ENDSTOP_HIT(Y, MIN); stepper.endstop_triggered(_AXIS(Y)); } else if (stepper.current_block->steps[_AXIS(Y)] > 0) { _ENDSTOP_HIT(Y, MIN); planner.endstop_triggered(_AXIS(Y)); }
else if (stepper.current_block->steps[_AXIS(Z)] > 0) { _ENDSTOP_HIT(Z, MIN); stepper.endstop_triggered(_AXIS(Z)); } else if (stepper.current_block->steps[_AXIS(Z)] > 0) { _ENDSTOP_HIT(Z, MIN); planner.endstop_triggered(_AXIS(Z)); }
G38_endstop_hit = true; G38_endstop_hit = true;
} }
} }

View file

@ -644,7 +644,8 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
#if ENABLED(SCARA_FEEDRATE_SCALING) #if ENABLED(SCARA_FEEDRATE_SCALING)
// For SCARA scale the feed rate from mm/s to degrees/s // For SCARA scale the feed rate from mm/s to degrees/s
// i.e., Complete the angular vector in the given time. // i.e., Complete the angular vector in the given time.
planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], raw[Z_AXIS], raw[E_AXIS], HYPOT(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB) * inverse_secs, active_extruder); if (!planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], raw[Z_AXIS], raw[E_AXIS], HYPOT(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB) * inverse_secs, active_extruder))
break;
/* /*
SERIAL_ECHO(segments); SERIAL_ECHO(segments);
SERIAL_ECHOPAIR(": X=", raw[X_AXIS]); SERIAL_ECHOPAIR(" Y=", raw[Y_AXIS]); SERIAL_ECHOPAIR(": X=", raw[X_AXIS]); SERIAL_ECHOPAIR(" Y=", raw[Y_AXIS]);
@ -654,7 +655,8 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
//*/ //*/
oldA = delta[A_AXIS]; oldB = delta[B_AXIS]; oldA = delta[A_AXIS]; oldB = delta[B_AXIS];
#else #else
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], _feedrate_mm_s, active_extruder, cartesian_segment_mm); if (!planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], _feedrate_mm_s, active_extruder, cartesian_segment_mm))
break;
#endif #endif
} }
@ -746,7 +748,8 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
idle(); idle();
} }
LOOP_XYZE(i) raw[i] += segment_distance[i]; LOOP_XYZE(i) raw[i] += segment_distance[i];
planner.buffer_line_kinematic(raw, fr_mm_s, active_extruder, cartesian_segment_mm); if (!planner.buffer_line_kinematic(raw, fr_mm_s, active_extruder, cartesian_segment_mm))
break;
} }
// Since segment_distance is only approximate, // Since segment_distance is only approximate,
@ -848,14 +851,14 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
} }
// unpark extruder: 1) raise, 2) move into starting XY position, 3) lower // unpark extruder: 1) raise, 2) move into starting XY position, 3) lower
for (uint8_t i = 0; i < 3; i++) for (uint8_t i = 0; i < 3; i++)
planner.buffer_line( if (!planner.buffer_line(
i == 0 ? raised_parked_position[X_AXIS] : current_position[X_AXIS], i == 0 ? raised_parked_position[X_AXIS] : current_position[X_AXIS],
i == 0 ? raised_parked_position[Y_AXIS] : current_position[Y_AXIS], i == 0 ? raised_parked_position[Y_AXIS] : current_position[Y_AXIS],
i == 2 ? current_position[Z_AXIS] : raised_parked_position[Z_AXIS], i == 2 ? current_position[Z_AXIS] : raised_parked_position[Z_AXIS],
current_position[E_AXIS], current_position[E_AXIS],
i == 1 ? PLANNER_XY_FEEDRATE() : planner.max_feedrate_mm_s[Z_AXIS], i == 1 ? PLANNER_XY_FEEDRATE() : planner.max_feedrate_mm_s[Z_AXIS],
active_extruder active_extruder)
); ) break;
delayed_move_time = 0; delayed_move_time = 0;
active_extruder_parked = false; active_extruder_parked = false;
#if ENABLED(DEBUG_LEVELING_FEATURE) #if ENABLED(DEBUG_LEVELING_FEATURE)
@ -872,11 +875,11 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
#endif #endif
// move duplicate extruder into correct duplication position. // move duplicate extruder into correct duplication position.
planner.set_position_mm(inactive_extruder_x_pos, current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]); planner.set_position_mm(inactive_extruder_x_pos, current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
planner.buffer_line( if (!planner.buffer_line(
current_position[X_AXIS] + duplicate_extruder_x_offset, current_position[X_AXIS] + duplicate_extruder_x_offset,
current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS],
planner.max_feedrate_mm_s[X_AXIS], 1 planner.max_feedrate_mm_s[X_AXIS], 1)
); ) break;
planner.synchronize(); planner.synchronize();
SYNC_PLAN_POSITION_KINEMATIC(); SYNC_PLAN_POSITION_KINEMATIC();
extruder_duplication_enabled = true; extruder_duplication_enabled = true;

View file

@ -100,13 +100,18 @@ Planner planner;
* A ring buffer of moves described in steps * A ring buffer of moves described in steps
*/ */
block_t Planner::block_buffer[BLOCK_BUFFER_SIZE]; block_t Planner::block_buffer[BLOCK_BUFFER_SIZE];
volatile uint8_t Planner::block_buffer_head, // Index of the next block to be pushed volatile uint8_t Planner::block_buffer_head, // Index of the next block to be pushed
Planner::block_buffer_tail; Planner::block_buffer_tail; // Index of the busy block, if any
uint16_t Planner::cleaning_buffer_counter; // A counter to disable queuing of blocks
float Planner::max_feedrate_mm_s[XYZE_N], // Max speeds in mm per second float Planner::max_feedrate_mm_s[XYZE_N], // Max speeds in mm per second
Planner::axis_steps_per_mm[XYZE_N], Planner::axis_steps_per_mm[XYZE_N],
Planner::steps_to_mm[XYZE_N]; Planner::steps_to_mm[XYZE_N];
#if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
bool Planner::abort_on_endstop_hit = false;
#endif
#if ENABLED(DISTINCT_E_FACTORS) #if ENABLED(DISTINCT_E_FACTORS)
uint8_t Planner::last_extruder = 0; // Respond to extruder change uint8_t Planner::last_extruder = 0; // Respond to extruder change
#endif #endif
@ -175,7 +180,7 @@ int32_t Planner::position[NUM_AXIS] = { 0 };
uint32_t Planner::cutoff_long; uint32_t Planner::cutoff_long;
float Planner::previous_speed[NUM_AXIS], float Planner::previous_speed[NUM_AXIS],
Planner::previous_nominal_speed; Planner::previous_nominal_speed_sqr;
#if ENABLED(DISABLE_INACTIVE_EXTRUDER) #if ENABLED(DISABLE_INACTIVE_EXTRUDER)
uint8_t Planner::g_uc_extruder_last_move[EXTRUDERS] = { 0 }; uint8_t Planner::g_uc_extruder_last_move[EXTRUDERS] = { 0 };
@ -212,7 +217,7 @@ void Planner::init() {
ZERO(position_float); ZERO(position_float);
#endif #endif
ZERO(previous_speed); ZERO(previous_speed);
previous_nominal_speed = 0.0; previous_nominal_speed_sqr = 0.0;
#if ABL_PLANAR #if ABL_PLANAR
bed_level_matrix.set_to_identity(); bed_level_matrix.set_to_identity();
#endif #endif
@ -363,7 +368,7 @@ void Planner::init() {
// //
static uint32_t get_period_inverse(uint32_t d) { static uint32_t get_period_inverse(uint32_t d) {
static const uint8_t inv_tab[256] PROGMEM = { static const uint8_t inv_tab[256] PROGMEM = {
255,253,252,250,248,246,244,242,240,238,236,234,233,231,229,227, 255,253,252,250,248,246,244,242,240,238,236,234,233,231,229,227,
225,224,222,220,218,217,215,213,212,210,208,207,205,203,202,200, 225,224,222,220,218,217,215,213,212,210,208,207,205,203,202,200,
199,197,195,194,192,191,189,188,186,185,183,182,180,179,178,176, 199,197,195,194,192,191,189,188,186,185,183,182,180,179,178,176,
@ -727,12 +732,9 @@ void Planner::init() {
} }
#else #else
// All the other 32 CPUs can easily perform the inverse using hardware division, // All the other 32 CPUs can easily perform the inverse using hardware division,
// so we don´t need to reduce precision or to use assembly language at all. // so we don't need to reduce precision or to use assembly language at all.
// This routine, for all the other archs, returns 0x100000000 / d ~= 0xFFFFFFFF / d // This routine, for all the other archs, returns 0x100000000 / d ~= 0xFFFFFFFF / d
static FORCE_INLINE uint32_t get_period_inverse(uint32_t d) { static FORCE_INLINE uint32_t get_period_inverse(const uint32_t d) { return 0xFFFFFFFF / d; }
return 0xFFFFFFFF / d;
}
#endif #endif
#endif #endif
@ -743,6 +745,7 @@ void Planner::init() {
* by the provided factors. * by the provided factors.
*/ */
void Planner::calculate_trapezoid_for_block(block_t* const block, const float &entry_factor, const float &exit_factor) { void Planner::calculate_trapezoid_for_block(block_t* const block, const float &entry_factor, const float &exit_factor) {
uint32_t initial_rate = CEIL(block->nominal_rate * entry_factor), uint32_t initial_rate = CEIL(block->nominal_rate * entry_factor),
final_rate = CEIL(block->nominal_rate * exit_factor); // (steps per second) final_rate = CEIL(block->nominal_rate * exit_factor); // (steps per second)
@ -757,19 +760,18 @@ void Planner::calculate_trapezoid_for_block(block_t* const block, const float &e
const int32_t accel = block->acceleration_steps_per_s2; const int32_t accel = block->acceleration_steps_per_s2;
// Steps required for acceleration, deceleration to/from nominal rate // Steps required for acceleration, deceleration to/from nominal rate
int32_t accelerate_steps = CEIL(estimate_acceleration_distance(initial_rate, block->nominal_rate, accel)), uint32_t accelerate_steps = CEIL(estimate_acceleration_distance(initial_rate, block->nominal_rate, accel)),
decelerate_steps = FLOOR(estimate_acceleration_distance(block->nominal_rate, final_rate, -accel)), decelerate_steps = FLOOR(estimate_acceleration_distance(block->nominal_rate, final_rate, -accel));
// Steps between acceleration and deceleration, if any // Steps between acceleration and deceleration, if any
plateau_steps = block->step_event_count - accelerate_steps - decelerate_steps; int32_t plateau_steps = block->step_event_count - accelerate_steps - decelerate_steps;
// Does accelerate_steps + decelerate_steps exceed step_event_count? // Does accelerate_steps + decelerate_steps exceed step_event_count?
// Then we can't possibly reach the nominal rate, there will be no cruising. // Then we can't possibly reach the nominal rate, there will be no cruising.
// Use intersection_distance() to calculate accel / braking time in order to // Use intersection_distance() to calculate accel / braking time in order to
// reach the final_rate exactly at the end of this block. // reach the final_rate exactly at the end of this block.
if (plateau_steps < 0) { if (plateau_steps < 0) {
accelerate_steps = CEIL(intersection_distance(initial_rate, final_rate, accel, block->step_event_count)); const float accelerate_steps_float = CEIL(intersection_distance(initial_rate, final_rate, accel, block->step_event_count));
NOLESS(accelerate_steps, 0); // Check limits due to numerical round-off accelerate_steps = MIN(uint32_t(MAX(accelerate_steps_float, 0)), block->step_event_count);
accelerate_steps = min((uint32_t)accelerate_steps, block->step_event_count);//(We can cast here to unsigned, because the above line ensures that we are above zero)
plateau_steps = 0; plateau_steps = 0;
#if ENABLED(BEZIER_JERK_CONTROL) #if ENABLED(BEZIER_JERK_CONTROL)
@ -796,7 +798,10 @@ void Planner::calculate_trapezoid_for_block(block_t* const block, const float &e
#endif #endif
CRITICAL_SECTION_START; // Fill variables used by the stepper in a critical section // Fill variables used by the stepper in a critical section
const bool was_enabled = STEPPER_ISR_ENABLED();
if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
if (!TEST(block->flag, BLOCK_BIT_BUSY)) { // Don't update variables if block is busy. if (!TEST(block->flag, BLOCK_BIT_BUSY)) { // Don't update variables if block is busy.
block->accelerate_until = accelerate_steps; block->accelerate_until = accelerate_steps;
block->decelerate_after = accelerate_steps + plateau_steps; block->decelerate_after = accelerate_steps + plateau_steps;
@ -810,32 +815,35 @@ void Planner::calculate_trapezoid_for_block(block_t* const block, const float &e
#endif #endif
block->final_rate = final_rate; block->final_rate = final_rate;
} }
CRITICAL_SECTION_END; if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
} }
// "Junction jerk" in this context is the immediate change in speed at the junction of two blocks.
// This method will calculate the junction jerk as the euclidean distance between the nominal
// velocities of the respective blocks.
//inline float junction_jerk(block_t *before, block_t *after) {
// return SQRT(
// POW((before->speed_x-after->speed_x), 2)+POW((before->speed_y-after->speed_y), 2));
//}
// The kernel called by recalculate() when scanning the plan from last to first entry. // The kernel called by recalculate() when scanning the plan from last to first entry.
void Planner::reverse_pass_kernel(block_t* const current, const block_t* const next) { void Planner::reverse_pass_kernel(block_t* const current, const block_t * const next) {
if (current && next) { if (current) {
// If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising. // If entry speed is already at the maximum entry speed, and there was no change of speed
// If not, block in state of acceleration or deceleration. Reset entry speed to maximum and // in the next block, there is no need to recheck. Block is cruising and there is no need to
// check for maximum allowable speed reductions to ensure maximum possible planned speed. // compute anything for this block,
const float max_entry_speed = current->max_entry_speed; // If not, block entry speed needs to be recalculated to ensure maximum possible planned speed.
if (current->entry_speed != max_entry_speed || TEST(next->flag, BLOCK_BIT_RECALCULATE)) { const float max_entry_speed_sqr = current->max_entry_speed_sqr;
// If nominal length true, max junction speed is guaranteed to be reached. Only compute
// for max allowable speed if block is decelerating and nominal length is false. // Compute maximum entry speed decelerating over the current block from its exit speed.
const float new_entry_speed = (TEST(current->flag, BLOCK_BIT_NOMINAL_LENGTH) || max_entry_speed <= next->entry_speed) // If not at the maximum entry speed, or the previous block entry speed changed
? max_entry_speed if (current->entry_speed_sqr != max_entry_speed_sqr || (next && TEST(next->flag, BLOCK_BIT_RECALCULATE))) {
: MIN(max_entry_speed, max_allowable_speed(-current->acceleration, next->entry_speed, current->millimeters));
if (new_entry_speed != current->entry_speed) { // If nominal length true, max junction speed is guaranteed to be reached.
current->entry_speed = new_entry_speed; // If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
// the current block and next block junction speeds are guaranteed to always be at their maximum
// junction speeds in deceleration and acceleration, respectively. This is due to how the current
// block nominal speed limits both the current and next maximum junction speeds. Hence, in both
// the reverse and forward planners, the corresponding block junction speed will always be at the
// the maximum junction speed and may always be ignored for any speed reduction checks.
const float new_entry_speed_sqr = TEST(current->flag, BLOCK_BIT_NOMINAL_LENGTH)
? max_entry_speed_sqr
: MIN(max_entry_speed_sqr, max_allowable_speed_sqr(-current->acceleration, next ? next->entry_speed_sqr : sq(MINIMUM_PLANNER_SPEED), current->millimeters));
if (current->entry_speed_sqr != new_entry_speed_sqr) {
current->entry_speed_sqr = new_entry_speed_sqr;
SBI(current->flag, BLOCK_BIT_RECALCULATE); SBI(current->flag, BLOCK_BIT_RECALCULATE);
} }
} }
@ -850,44 +858,37 @@ void Planner::reverse_pass() {
if (movesplanned() > 2) { if (movesplanned() > 2) {
const uint8_t endnr = next_block_index(block_buffer_tail); // tail is running. tail+1 shouldn't be altered because it's connected to the running block. const uint8_t endnr = next_block_index(block_buffer_tail); // tail is running. tail+1 shouldn't be altered because it's connected to the running block.
uint8_t blocknr = prev_block_index(block_buffer_head); uint8_t blocknr = prev_block_index(block_buffer_head);
block_t* current = &block_buffer[blocknr];
// Last/newest block in buffer: // Perform the reverse pass
const float max_entry_speed = current->max_entry_speed; block_t *current, *next = NULL;
if (current->entry_speed != max_entry_speed) { while (blocknr != endnr) {
// If nominal length true, max junction speed is guaranteed to be reached. Only compute // Perform the reverse pass - Only consider non sync blocks
// for max allowable speed if block is decelerating and nominal length is false.
const float new_entry_speed = TEST(current->flag, BLOCK_BIT_NOMINAL_LENGTH)
? max_entry_speed
: MIN(max_entry_speed, max_allowable_speed(-current->acceleration, MINIMUM_PLANNER_SPEED, current->millimeters));
if (current->entry_speed != new_entry_speed) {
current->entry_speed = new_entry_speed;
SBI(current->flag, BLOCK_BIT_RECALCULATE);
}
}
do {
const block_t * const next = current;
blocknr = prev_block_index(blocknr);
current = &block_buffer[blocknr]; current = &block_buffer[blocknr];
reverse_pass_kernel(current, next); if (!TEST(current->flag, BLOCK_BIT_SYNC_POSITION)) {
} while (blocknr != endnr); reverse_pass_kernel(current, next);
next = current;
}
// Advance to the next
blocknr = prev_block_index(blocknr);
}
} }
} }
// The kernel called by recalculate() when scanning the plan from first to last entry. // The kernel called by recalculate() when scanning the plan from first to last entry.
void Planner::forward_pass_kernel(const block_t* const previous, block_t* const current) { void Planner::forward_pass_kernel(const block_t * const previous, block_t* const current) {
if (previous) { if (previous) {
// If the previous block is an acceleration block, too short to complete the full speed // If the previous block is an acceleration block, too short to complete the full speed
// change, adjust the entry speed accordingly. Entry speeds have already been reset, // change, adjust the entry speed accordingly. Entry speeds have already been reset,
// maximized, and reverse-planned. If nominal length is set, max junction speed is // maximized, and reverse-planned. If nominal length is set, max junction speed is
// guaranteed to be reached. No need to recheck. // guaranteed to be reached. No need to recheck.
if (!TEST(previous->flag, BLOCK_BIT_NOMINAL_LENGTH)) { if (!TEST(previous->flag, BLOCK_BIT_NOMINAL_LENGTH)) {
if (previous->entry_speed < current->entry_speed) { if (previous->entry_speed_sqr < current->entry_speed_sqr) {
const float new_entry_speed = MIN(current->entry_speed, max_allowable_speed(-previous->acceleration, previous->entry_speed, previous->millimeters)); // Compute the maximum allowable speed
// Check for junction speed change const float new_entry_speed_sqr = max_allowable_speed_sqr(-previous->acceleration, previous->entry_speed_sqr, previous->millimeters);
if (current->entry_speed != new_entry_speed) { // If true, current block is full-acceleration
current->entry_speed = new_entry_speed; if (current->entry_speed_sqr > new_entry_speed_sqr) {
// Always <= max_entry_speed_sqr. Backward pass sets this.
current->entry_speed_sqr = new_entry_speed_sqr;
SBI(current->flag, BLOCK_BIT_RECALCULATE); SBI(current->flag, BLOCK_BIT_RECALCULATE);
} }
} }
@ -900,15 +901,21 @@ void Planner::forward_pass_kernel(const block_t* const previous, block_t* const
* Once in reverse and once forward. This implements the forward pass. * Once in reverse and once forward. This implements the forward pass.
*/ */
void Planner::forward_pass() { void Planner::forward_pass() {
block_t* block[3] = { NULL, NULL, NULL }; const uint8_t endnr = block_buffer_head;
uint8_t blocknr = block_buffer_tail;
for (uint8_t b = block_buffer_tail; b != block_buffer_head; b = next_block_index(b)) { // Perform the forward pass
block[0] = block[1]; block_t *current, *previous = NULL;
block[1] = block[2]; while (blocknr != endnr) {
block[2] = &block_buffer[b]; // Perform the forward pass - Only consider non-sync blocks
forward_pass_kernel(block[0], block[1]); current = &block_buffer[blocknr];
if (!TEST(current->flag, BLOCK_BIT_SYNC_POSITION)) {
forward_pass_kernel(previous, current);
previous = current;
}
// Advance to the previous
blocknr = next_block_index(blocknr);
} }
forward_pass_kernel(block[1], block[2]);
} }
/** /**
@ -917,38 +924,72 @@ void Planner::forward_pass() {
* recalculate() after updating the blocks. * recalculate() after updating the blocks.
*/ */
void Planner::recalculate_trapezoids() { void Planner::recalculate_trapezoids() {
int8_t block_index = block_buffer_tail; uint8_t block_index = block_buffer_tail;
block_t *current, *next = NULL;
// As there could be a sync block in the head of the queue, and the next loop must not
// recalculate the head block (as it needs to be specially handled), scan backwards until
// we find the first non SYNC block
uint8_t head_block_index = block_buffer_head;
while (head_block_index != block_index) {
// Go back (head always point to the first free block)
uint8_t prev_index = prev_block_index(head_block_index);
// Get the pointer to the block
block_t *prev = &block_buffer[prev_index];
// If not dealing with a sync block, we are done. The last block is not a SYNC block
if (!TEST(prev->flag, BLOCK_BIT_SYNC_POSITION)) break;
// Examine the previous block. This and all following are SYNC blocks
head_block_index = prev_index;
};
// Go from the tail (currently executed block) to the first block, without including it)
block_t *current = NULL, *next = NULL;
float current_entry_speed = 0.0, next_entry_speed = 0.0;
while (block_index != head_block_index) {
while (block_index != block_buffer_head) {
current = next;
next = &block_buffer[block_index]; next = &block_buffer[block_index];
if (current) {
// Recalculate if current block entry or exit junction speed has changed. // Skip sync blocks
if (TEST(current->flag, BLOCK_BIT_RECALCULATE) || TEST(next->flag, BLOCK_BIT_RECALCULATE)) { if (!TEST(next->flag, BLOCK_BIT_SYNC_POSITION)) {
// NOTE: Entry and exit factors always > 0 by all previous logic operations. next_entry_speed = SQRT(next->entry_speed_sqr);
const float nomr = 1.0 / current->nominal_speed;
calculate_trapezoid_for_block(current, current->entry_speed * nomr, next->entry_speed * nomr); if (current) {
#if ENABLED(LIN_ADVANCE) // Recalculate if current block entry or exit junction speed has changed.
if (current->use_advance_lead) { if (TEST(current->flag, BLOCK_BIT_RECALCULATE) || TEST(next->flag, BLOCK_BIT_RECALCULATE)) {
const float comp = current->e_D_ratio * extruder_advance_K * axis_steps_per_mm[E_AXIS]; // NOTE: Entry and exit factors always > 0 by all previous logic operations.
current->max_adv_steps = current->nominal_speed * comp; const float current_nominal_speed = SQRT(current->nominal_speed_sqr),
current->final_adv_steps = next->entry_speed * comp; nomr = 1.0 / current_nominal_speed;
} calculate_trapezoid_for_block(current, current_entry_speed * nomr, next_entry_speed * nomr);
#endif #if ENABLED(LIN_ADVANCE)
CBI(current->flag, BLOCK_BIT_RECALCULATE); // Reset current only to ensure next trapezoid is computed if (current->use_advance_lead) {
const float comp = current->e_D_ratio * extruder_advance_K * axis_steps_per_mm[E_AXIS];
current->max_adv_steps = current_nominal_speed * comp;
current->final_adv_steps = next_entry_speed * comp;
}
#endif
CBI(current->flag, BLOCK_BIT_RECALCULATE); // Reset current only to ensure next trapezoid is computed
}
} }
current = next;
current_entry_speed = next_entry_speed;
} }
block_index = next_block_index(block_index); block_index = next_block_index(block_index);
} }
// Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated. // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated.
if (next) { if (next) {
const float nomr = 1.0 / next->nominal_speed; const float next_nominal_speed = SQRT(next->nominal_speed_sqr),
calculate_trapezoid_for_block(next, next->entry_speed * nomr, (MINIMUM_PLANNER_SPEED) * nomr); nomr = 1.0 / next_nominal_speed;
calculate_trapezoid_for_block(next, next_entry_speed * nomr, (MINIMUM_PLANNER_SPEED) * nomr);
#if ENABLED(LIN_ADVANCE) #if ENABLED(LIN_ADVANCE)
if (next->use_advance_lead) { if (next->use_advance_lead) {
const float comp = next->e_D_ratio * extruder_advance_K * axis_steps_per_mm[E_AXIS]; const float comp = next->e_D_ratio * extruder_advance_K * axis_steps_per_mm[E_AXIS];
next->max_adv_steps = next->nominal_speed * comp; next->max_adv_steps = next_nominal_speed * comp;
next->final_adv_steps = (MINIMUM_PLANNER_SPEED) * comp; next->final_adv_steps = (MINIMUM_PLANNER_SPEED) * comp;
} }
#endif #endif
@ -998,7 +1039,7 @@ void Planner::recalculate() {
for (uint8_t b = block_buffer_tail; b != block_buffer_head; b = next_block_index(b)) { for (uint8_t b = block_buffer_tail; b != block_buffer_head; b = next_block_index(b)) {
block_t* block = &block_buffer[b]; block_t* block = &block_buffer[b];
if (block->steps[X_AXIS] || block->steps[Y_AXIS] || block->steps[Z_AXIS]) { if (block->steps[X_AXIS] || block->steps[Y_AXIS] || block->steps[Z_AXIS]) {
float se = (float)block->steps[E_AXIS] / block->step_event_count * block->nominal_speed; // mm/sec; const float se = (float)block->steps[E_AXIS] / block->step_event_count * SQRT(block->nominal_speed_sqr); // mm/sec;
NOLESS(high, se); NOLESS(high, se);
} }
} }
@ -1299,6 +1340,59 @@ void Planner::check_axes_activity() {
#endif // PLANNER_LEVELING #endif // PLANNER_LEVELING
void Planner::quick_stop() {
// Remove all the queued blocks. Note that this function is NOT
// called from the Stepper ISR, so we must consider tail as readonly!
// that is why we set head to tail!
block_buffer_head = block_buffer_tail;
#if ENABLED(ULTRA_LCD)
// Clear the accumulated runtime
clear_block_buffer_runtime();
#endif
// Make sure to drop any attempt of queuing moves for at least 1 second
cleaning_buffer_counter = 1000;
// And stop the stepper ISR
stepper.quick_stop();
}
void Planner::endstop_triggered(const AxisEnum axis) {
/*NB: This will be called via endstops.update()
and endstops.update() can be called from the temperature
ISR. So Stepper interrupts are enabled */
// Disable stepper ISR
bool stepper_isr_enabled = STEPPER_ISR_ENABLED();
DISABLE_STEPPER_DRIVER_INTERRUPT();
// Record stepper position
stepper.endstop_triggered(axis);
// Discard the active block that led to the trigger
discard_current_block();
// Discard the CONTINUED block, if any. Note the planner can only queue 1 continued
// block after a previous non continued block, as the condition to queue them
// is that there are no queued blocks at the time a new block is queued.
const bool discard = has_blocks_queued() && TEST(block_buffer[block_buffer_tail].flag, BLOCK_BIT_CONTINUED);
if (discard) discard_current_block();
// Reenable stepper ISR if it was enabled
if (stepper_isr_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
}
float Planner::triggered_position_mm(const AxisEnum axis) {
return stepper.triggered_position(axis) * steps_to_mm[axis];
}
void Planner::finish_and_disable() {
while (has_blocks_queued() || cleaning_buffer_counter) idle();
disable_all_steppers();
}
/** /**
* Get an axis position according to stepper position(s) * Get an axis position according to stepper position(s)
* For CORE machines apply translation from ABC to XYZ. * For CORE machines apply translation from ABC to XYZ.
@ -1311,7 +1405,7 @@ float Planner::get_axis_position_mm(const AxisEnum axis) {
// Protect the access to the position. // Protect the access to the position.
const bool was_enabled = STEPPER_ISR_ENABLED(); const bool was_enabled = STEPPER_ISR_ENABLED();
DISABLE_STEPPER_DRIVER_INTERRUPT(); if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
// ((a1+a2)+(a1-a2))/2 -> (a1+a2+a1-a2)/2 -> (a1+a1)/2 -> a1 // ((a1+a2)+(a1-a2))/2 -> (a1+a2+a1-a2)/2 -> (a1+a1)/2 -> a1
// ((a1+a2)-(a1-a2))/2 -> (a1+a2-a1+a2)/2 -> (a2+a2)/2 -> a2 // ((a1+a2)-(a1-a2))/2 -> (a1+a2-a1+a2)/2 -> (a2+a2)/2 -> a2
@ -1333,18 +1427,69 @@ float Planner::get_axis_position_mm(const AxisEnum axis) {
/** /**
* Block until all buffered steps are executed / cleaned * Block until all buffered steps are executed / cleaned
*/ */
void Planner::synchronize() { while (has_blocks_queued() || stepper.cleaning_buffer_counter) idle(); } void Planner::synchronize() { while (has_blocks_queued() || cleaning_buffer_counter) idle(); }
/** /**
* Planner::_buffer_steps * Planner::_buffer_steps
* *
* Add a new linear movement to the buffer (in terms of steps). * Add a new linear movement to the planner queue (in terms of steps).
* *
* target - target position in steps units * target - target position in steps units
* fr_mm_s - (target) speed of the move * fr_mm_s - (target) speed of the move
* extruder - target extruder * extruder - target extruder
* millimeters - the length of the movement, if known
*
* Returns true if movement was properly queued, false otherwise
*/ */
void Planner::_buffer_steps(const int32_t (&target)[XYZE] bool Planner::_buffer_steps(const int32_t (&target)[XYZE]
#if HAS_POSITION_FLOAT
, const float (&target_float)[XYZE]
#endif
, float fr_mm_s, const uint8_t extruder, const float &millimeters
) {
// If we are cleaning, do not accept queuing of movements
if (cleaning_buffer_counter) return false;
// Wait for the next available block
uint8_t next_buffer_head;
block_t * const block = get_next_free_block(next_buffer_head);
// Fill the block with the specified movement
if (!_populate_block(block, false, target
#if HAS_POSITION_FLOAT
, target_float
#endif
, fr_mm_s, extruder, millimeters
)) {
// Movement was not queued, probably because it was too short.
// Simply accept that as movement queued and done
return true;
}
// Move buffer head
block_buffer_head = next_buffer_head;
// Recalculate and optimize trapezoidal speed profiles
recalculate();
// Movement successfully queued!
return true;
}
/**
* Planner::_populate_block
*
* Fills a new linear movement in the block (in terms of steps).
*
* target - target position in steps units
* fr_mm_s - (target) speed of the move
* extruder - target extruder
*
* Returns true is movement is acceptable, false otherwise
*/
bool Planner::_populate_block(block_t * const block, bool split_move,
const int32_t (&target)[XYZE]
#if HAS_POSITION_FLOAT #if HAS_POSITION_FLOAT
, const float (&target_float)[XYZE] , const float (&target_float)[XYZE]
#endif #endif
@ -1358,7 +1503,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
int32_t de = target[E_AXIS] - position[E_AXIS]; int32_t de = target[E_AXIS] - position[E_AXIS];
/* <-- add a slash to enable /* <-- add a slash to enable
SERIAL_ECHOPAIR(" _buffer_steps FR:", fr_mm_s); SERIAL_ECHOPAIR(" _populate_block FR:", fr_mm_s);
SERIAL_ECHOPAIR(" A:", target[A_AXIS]); SERIAL_ECHOPAIR(" A:", target[A_AXIS]);
SERIAL_ECHOPAIR(" (", da); SERIAL_ECHOPAIR(" (", da);
SERIAL_ECHOPAIR(" steps) B:", target[B_AXIS]); SERIAL_ECHOPAIR(" steps) B:", target[B_AXIS]);
@ -1427,10 +1572,6 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
const float esteps_float = de * e_factor[extruder]; const float esteps_float = de * e_factor[extruder];
const int32_t esteps = ABS(esteps_float) + 0.5; const int32_t esteps = ABS(esteps_float) + 0.5;
// Wait for the next available block
uint8_t next_buffer_head;
block_t * const block = get_next_free_block(next_buffer_head);
// Clear all flags, including the "busy" bit // Clear all flags, including the "busy" bit
block->flag = 0x00; block->flag = 0x00;
@ -1466,7 +1607,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
block->step_event_count = MAX4(block->steps[A_AXIS], block->steps[B_AXIS], block->steps[C_AXIS], esteps); block->step_event_count = MAX4(block->steps[A_AXIS], block->steps[B_AXIS], block->steps[C_AXIS], esteps);
// Bail if this is a zero-length block // Bail if this is a zero-length block
if (block->step_event_count < MIN_STEPS_PER_SEGMENT) return; if (block->step_event_count < MIN_STEPS_PER_SEGMENT) return false;
// For a mixing extruder, get a magnified step_event_count for each // For a mixing extruder, get a magnified step_event_count for each
#if ENABLED(MIXING_EXTRUDER) #if ENABLED(MIXING_EXTRUDER)
@ -1706,12 +1847,16 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
#endif #endif
#if ENABLED(ULTRA_LCD) #if ENABLED(ULTRA_LCD)
CRITICAL_SECTION_START // Protect the access to the position.
block_buffer_runtime_us += segment_time_us; const bool was_enabled = STEPPER_ISR_ENABLED();
CRITICAL_SECTION_END if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
block_buffer_runtime_us += segment_time_us;
if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
#endif #endif
block->nominal_speed = block->millimeters * inverse_secs; // (mm/sec) Always > 0 block->nominal_speed_sqr = sq(block->millimeters * inverse_secs); // (mm/sec)^2 Always > 0
block->nominal_rate = CEIL(block->step_event_count * inverse_secs); // (step/sec) Always > 0 block->nominal_rate = CEIL(block->step_event_count * inverse_secs); // (step/sec) Always > 0
#if ENABLED(FILAMENT_WIDTH_SENSOR) #if ENABLED(FILAMENT_WIDTH_SENSOR)
@ -1799,8 +1944,8 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
// Correct the speed // Correct the speed
if (speed_factor < 1.0) { if (speed_factor < 1.0) {
LOOP_XYZE(i) current_speed[i] *= speed_factor; LOOP_XYZE(i) current_speed[i] *= speed_factor;
block->nominal_speed *= speed_factor;
block->nominal_rate *= speed_factor; block->nominal_rate *= speed_factor;
block->nominal_speed_sqr = block->nominal_speed_sqr * sq(speed_factor);
} }
// Compute and limit the acceleration rate for the trapezoid generator. // Compute and limit the acceleration rate for the trapezoid generator.
@ -1895,13 +2040,13 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
block->acceleration_steps_per_s2 = accel; block->acceleration_steps_per_s2 = accel;
block->acceleration = accel / steps_per_mm; block->acceleration = accel / steps_per_mm;
#if DISABLED(BEZIER_JERK_CONTROL) #if DISABLED(BEZIER_JERK_CONTROL)
block->acceleration_rate = (long)(accel * (4096.0 * 4096.0 / (HAL_STEPPER_TIMER_RATE))); block->acceleration_rate = (uint32_t)(accel * (4096.0 * 4096.0 / (HAL_STEPPER_TIMER_RATE)));
#endif #endif
#if ENABLED(LIN_ADVANCE) #if ENABLED(LIN_ADVANCE)
if (block->use_advance_lead) { if (block->use_advance_lead) {
block->advance_speed = (HAL_STEPPER_TIMER_RATE) / (extruder_advance_K * block->e_D_ratio * block->acceleration * axis_steps_per_mm[E_AXIS_N]); block->advance_speed = (HAL_STEPPER_TIMER_RATE) / (extruder_advance_K * block->e_D_ratio * block->acceleration * axis_steps_per_mm[E_AXIS_N]);
#if ENABLED(LA_DEBUG) #if ENABLED(LA_DEBUG)
if (extruder_advance_K * block->e_D_ratio * block->acceleration * 2 < block->nominal_speed * block->e_D_ratio) if (extruder_advance_K * block->e_D_ratio * block->acceleration * 2 < SQRT(block->nominal_speed_sqr) * block->e_D_ratio)
SERIAL_ECHOLNPGM("More than 2 steps per eISR loop executed."); SERIAL_ECHOLNPGM("More than 2 steps per eISR loop executed.");
if (block->advance_speed < 200) if (block->advance_speed < 200)
SERIAL_ECHOLNPGM("eISR running at > 10kHz."); SERIAL_ECHOLNPGM("eISR running at > 10kHz.");
@ -1909,7 +2054,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
} }
#endif #endif
float vmax_junction; // Initial limit on the segment entry velocity float vmax_junction_sqr; // Initial limit on the segment entry velocity (mm/s)^2
#if ENABLED(JUNCTION_DEVIATION) #if ENABLED(JUNCTION_DEVIATION)
@ -1935,7 +2080,17 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
* changed dynamically during operation nor can the line move geometry. This must be kept in * changed dynamically during operation nor can the line move geometry. This must be kept in
* memory in the event of a feedrate override changing the nominal speeds of blocks, which can * memory in the event of a feedrate override changing the nominal speeds of blocks, which can
* change the overall maximum entry speed conditions of all blocks. * change the overall maximum entry speed conditions of all blocks.
*/ *
* #######
* https://github.com/MarlinFirmware/Marlin/issues/10341#issuecomment-388191754
*
* hoffbaked: on May 10 2018 tuned and improved the GRBL algorithm for Marlin:
Okay! It seems to be working good. I somewhat arbitrarily cut it off at 1mm
on then on anything with less sides than an octagon. With this, and the
reverse pass actually recalculating things, a corner acceleration value
of 1000 junction deviation of .05 are pretty reasonable. If the cycles
can be spared, a better acos could be used. For all I know, it may be
already calculated in a different place. */
// Unit vector of previous path line segment // Unit vector of previous path line segment
static float previous_unit_vec[ static float previous_unit_vec[
@ -1956,7 +2111,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
}; };
// Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles. // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
if (moves_queued && !UNEAR_ZERO(previous_nominal_speed)) { if (moves_queued && !UNEAR_ZERO(previous_nominal_speed_sqr)) {
// Compute cosine of angle between previous and current path. (prev_unit_vec is negative) // 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. // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
float junction_cos_theta = -previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] float junction_cos_theta = -previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
@ -1970,21 +2125,33 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
// NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta). // NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta).
if (junction_cos_theta > 0.999999) { if (junction_cos_theta > 0.999999) {
// For a 0 degree acute junction, just set minimum junction speed. // For a 0 degree acute junction, just set minimum junction speed.
vmax_junction = MINIMUM_PLANNER_SPEED; vmax_junction_sqr = sq(MINIMUM_PLANNER_SPEED);
} }
else { else {
junction_cos_theta = MAX(junction_cos_theta, -0.999999); // Check for numerical round-off to avoid divide by zero. NOLESS(junction_cos_theta, -0.999999); // Check for numerical round-off to avoid divide by zero.
const float sin_theta_d2 = SQRT(0.5 * (1.0 - junction_cos_theta)); // Trig half angle identity. Always positive. const float sin_theta_d2 = SQRT(0.5 * (1.0 - junction_cos_theta)); // Trig half angle identity. Always positive.
// TODO: Technically, the acceleration used in calculation needs to be limited by the minimum of the // TODO: Technically, the acceleration used in calculation needs to be limited by the minimum of the
// two junctions. However, this shouldn't be a significant problem except in extreme circumstances. // two junctions. However, this shouldn't be a significant problem except in extreme circumstances.
vmax_junction = SQRT((block->acceleration * JUNCTION_DEVIATION_FACTOR * sin_theta_d2) / (1.0 - sin_theta_d2)); vmax_junction_sqr = (JUNCTION_ACCELERATION_FACTOR * JUNCTION_DEVIATION_FACTOR * sin_theta_d2) / (1.0 - sin_theta_d2);
if (block->millimeters < 1.0) {
// Fast acos approximation, minus the error bar to be safe
float junction_theta = (RADIANS(-40) * sq(junction_cos_theta) - RADIANS(50)) * junction_cos_theta + RADIANS(90) - 0.18;
// If angle is greater than 135 degrees (octagon), find speed for approximate arc
if (junction_theta > RADIANS(135)) {
const float limit_sqr = block->millimeters / (RADIANS(180) - junction_theta) * JUNCTION_ACCELERATION_FACTOR;
NOMORE(vmax_junction_sqr, limit_sqr);
}
}
} }
vmax_junction = MIN3(vmax_junction, block->nominal_speed, previous_nominal_speed); // Get the lowest speed
vmax_junction_sqr = MIN3(vmax_junction_sqr, block->nominal_speed_sqr, previous_nominal_speed_sqr);
} }
else // Init entry speed to zero. Assume it starts from rest. Planner will correct this later. else // Init entry speed to zero. Assume it starts from rest. Planner will correct this later.
vmax_junction = 0.0; vmax_junction_sqr = 0.0;
COPY(previous_unit_vec, unit_vec); COPY(previous_unit_vec, unit_vec);
@ -2000,13 +2167,15 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
// Exit speed limited by a jerk to full halt of a previous last segment // Exit speed limited by a jerk to full halt of a previous last segment
static float previous_safe_speed; static float previous_safe_speed;
float safe_speed = block->nominal_speed; const float nominal_speed = SQRT(block->nominal_speed_sqr);
float safe_speed = nominal_speed;
uint8_t limited = 0; uint8_t limited = 0;
LOOP_XYZE(i) { LOOP_XYZE(i) {
const float jerk = ABS(current_speed[i]), maxj = max_jerk[i]; const float jerk = ABS(current_speed[i]), maxj = max_jerk[i];
if (jerk > maxj) { if (jerk > maxj) {
if (limited) { if (limited) {
const float mjerk = maxj * block->nominal_speed; const float mjerk = maxj * nominal_speed;
if (jerk * safe_speed > mjerk) safe_speed = mjerk / jerk; if (jerk * safe_speed > mjerk) safe_speed = mjerk / jerk;
} }
else { else {
@ -2016,19 +2185,21 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
} }
} }
if (moves_queued && !UNEAR_ZERO(previous_nominal_speed)) { float vmax_junction;
if (moves_queued && !UNEAR_ZERO(previous_nominal_speed_sqr)) {
// Estimate a maximum velocity allowed at a joint of two successive segments. // Estimate a maximum velocity allowed at a joint of two successive segments.
// If this maximum velocity allowed is lower than the minimum of the entry / exit safe velocities, // If this maximum velocity allowed is lower than the minimum of the entry / exit safe velocities,
// then the machine is not coasting anymore and the safe entry / exit velocities shall be used. // then the machine is not coasting anymore and the safe entry / exit velocities shall be used.
// The junction velocity will be shared between successive segments. Limit the junction velocity to their minimum.
// Pick the smaller of the nominal speeds. Higher speed shall not be achieved at the junction during coasting.
vmax_junction = MIN(block->nominal_speed, previous_nominal_speed);
// Factor to multiply the previous / current nominal velocities to get componentwise limited velocities. // Factor to multiply the previous / current nominal velocities to get componentwise limited velocities.
float v_factor = 1; float v_factor = 1;
limited = 0; limited = 0;
// The junction velocity will be shared between successive segments. Limit the junction velocity to their minimum.
// Pick the smaller of the nominal speeds. Higher speed shall not be achieved at the junction during coasting.
const float previous_nominal_speed = SQRT(previous_nominal_speed_sqr);
vmax_junction = MIN(nominal_speed, previous_nominal_speed);
// Now limit the jerk in all axes. // Now limit the jerk in all axes.
const float smaller_speed_factor = vmax_junction / previous_nominal_speed; const float smaller_speed_factor = vmax_junction / previous_nominal_speed;
LOOP_XYZE(axis) { LOOP_XYZE(axis) {
@ -2063,16 +2234,19 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
vmax_junction = safe_speed; vmax_junction = safe_speed;
previous_safe_speed = safe_speed; previous_safe_speed = safe_speed;
vmax_junction_sqr = sq(vmax_junction);
#endif // Classic Jerk Limiting #endif // Classic Jerk Limiting
// Max entry speed of this block equals the max exit speed of the previous block. // Max entry speed of this block equals the max exit speed of the previous block.
block->max_entry_speed = vmax_junction; block->max_entry_speed_sqr = vmax_junction_sqr;
// Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED. // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
const float v_allowable = max_allowable_speed(-block->acceleration, MINIMUM_PLANNER_SPEED, block->millimeters); const float v_allowable_sqr = max_allowable_speed_sqr(-block->acceleration, sq(MINIMUM_PLANNER_SPEED), block->millimeters);
// If stepper ISR is disabled, this indicates buffer_segment wants to add a split block.
// In this case start with the max. allowed speed to avoid an interrupted first move. // If we are trying to add a split block, start with the
block->entry_speed = STEPPER_ISR_ENABLED() ? MINIMUM_PLANNER_SPEED : MIN(vmax_junction, v_allowable); // max. allowed speed to avoid an interrupted first move.
block->entry_speed_sqr = !split_move ? sq(MINIMUM_PLANNER_SPEED) : MIN(vmax_junction_sqr, v_allowable_sqr);
// Initialize planner efficiency flags // Initialize planner efficiency flags
// Set flag if block will always reach maximum junction speed regardless of entry/exit speeds. // Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
@ -2082,25 +2256,22 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
// block nominal speed limits both the current and next maximum junction speeds. Hence, in both // block nominal speed limits both the current and next maximum junction speeds. Hence, in both
// the reverse and forward planners, the corresponding block junction speed will always be at the // the reverse and forward planners, the corresponding block junction speed will always be at the
// the maximum junction speed and may always be ignored for any speed reduction checks. // the maximum junction speed and may always be ignored for any speed reduction checks.
block->flag |= block->nominal_speed <= v_allowable ? BLOCK_FLAG_RECALCULATE | BLOCK_FLAG_NOMINAL_LENGTH : BLOCK_FLAG_RECALCULATE; block->flag |= block->nominal_speed_sqr <= v_allowable_sqr ? BLOCK_FLAG_RECALCULATE | BLOCK_FLAG_NOMINAL_LENGTH : BLOCK_FLAG_RECALCULATE;
// Update previous path unit_vector and nominal speed // Update previous path unit_vector and nominal speed
COPY(previous_speed, current_speed); COPY(previous_speed, current_speed);
previous_nominal_speed = block->nominal_speed; previous_nominal_speed_sqr = block->nominal_speed_sqr;
// Move buffer head // Update the position
block_buffer_head = next_buffer_head;
// Update the position (only when a move was queued)
static_assert(COUNT(target) > 1, "Parameter to _buffer_steps must be (&target)[XYZE]!"); static_assert(COUNT(target) > 1, "Parameter to _buffer_steps must be (&target)[XYZE]!");
COPY(position, target); COPY(position, target);
#if HAS_POSITION_FLOAT #if HAS_POSITION_FLOAT
COPY(position_float, target_float); COPY(position_float, target_float);
#endif #endif
recalculate(); // Movement was accepted
return true;
} // _buffer_steps() } // _populate_block()
/** /**
* Planner::buffer_sync_block * Planner::buffer_sync_block
@ -2111,29 +2282,15 @@ void Planner::buffer_sync_block() {
uint8_t next_buffer_head; uint8_t next_buffer_head;
block_t * const block = get_next_free_block(next_buffer_head); block_t * const block = get_next_free_block(next_buffer_head);
// Clear block
memset(block, 0, sizeof(block_t));
block->flag = BLOCK_FLAG_SYNC_POSITION; block->flag = BLOCK_FLAG_SYNC_POSITION;
block->steps[A_AXIS] = position[A_AXIS]; block->position[A_AXIS] = position[A_AXIS];
block->steps[B_AXIS] = position[B_AXIS]; block->position[B_AXIS] = position[B_AXIS];
block->steps[C_AXIS] = position[C_AXIS]; block->position[C_AXIS] = position[C_AXIS];
block->steps[E_AXIS] = position[E_AXIS]; block->position[E_AXIS] = position[E_AXIS];
#if ENABLED(LIN_ADVANCE)
block->use_advance_lead = false;
#endif
block->nominal_speed =
block->entry_speed =
block->max_entry_speed =
block->millimeters =
block->acceleration = 0;
block->step_event_count =
block->nominal_rate =
block->initial_rate =
block->final_rate =
block->acceleration_steps_per_s2 =
block->segment_time_us = 0;
block_buffer_head = next_buffer_head; block_buffer_head = next_buffer_head;
stepper.wake_up(); stepper.wake_up();
@ -2151,7 +2308,11 @@ void Planner::buffer_sync_block() {
* extruder - target extruder * extruder - target extruder
* millimeters - the length of the movement, if known * millimeters - the length of the movement, if known
*/ */
void Planner::buffer_segment(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder, const float &millimeters/*=0.0*/) { bool Planner::buffer_segment(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder, const float &millimeters/*=0.0*/) {
// If we are cleaning, do not accept queuing of movements
if (cleaning_buffer_counter) return false;
// When changing extruders recalculate steps corresponding to the E position // When changing extruders recalculate steps corresponding to the E position
#if ENABLED(DISTINCT_E_FACTORS) #if ENABLED(DISTINCT_E_FACTORS)
if (last_extruder != extruder && axis_steps_per_mm[E_AXIS_N] != axis_steps_per_mm[E_AXIS + last_extruder]) { if (last_extruder != extruder && axis_steps_per_mm[E_AXIS_N] != axis_steps_per_mm[E_AXIS + last_extruder]) {
@ -2220,37 +2381,80 @@ void Planner::buffer_segment(const float &a, const float &b, const float &c, con
const float between_float[ABCE] = { _BETWEEN_F(A), _BETWEEN_F(B), _BETWEEN_F(C), _BETWEEN_F(E) }; const float between_float[ABCE] = { _BETWEEN_F(A), _BETWEEN_F(B), _BETWEEN_F(C), _BETWEEN_F(E) };
#endif #endif
DISABLE_STEPPER_DRIVER_INTERRUPT(); // The new head value is not assigned yet
uint8_t buffer_head = 0;
bool added = false;
_buffer_steps(between uint8_t next_buffer_head;
block_t *block = get_next_free_block(next_buffer_head, 2);
// Fill the block with the specified movement
if (
_populate_block(block, true, between
#if HAS_POSITION_FLOAT
, between_float
#endif
, fr_mm_s, extruder, millimeters * 0.5
)
) {
// Movement accepted - Point to the next reserved block
block = &block_buffer[next_buffer_head];
// Store into the new to be stored head
buffer_head = next_buffer_head;
added = true;
// And advance the pointer to the next unused slot
next_buffer_head = next_block_index(next_buffer_head);
}
// Fill the second part of the block with the 2nd part of the movement
if (
_populate_block(block, true, target
#if HAS_POSITION_FLOAT
, target_float
#endif
, fr_mm_s, extruder, millimeters * 0.5
)
) {
// Movement accepted - If this block is a continuation
// of the previous one, mark it as such
if (added) SBI(block->flag, BLOCK_BIT_CONTINUED);
// Store into the new to be stored head
buffer_head = next_buffer_head;
added = true;
}
// If any of the movements was added
if (added) {
// Move buffer head and add all the blocks that were filled
// successfully to the movement queue.
block_buffer_head = buffer_head;
// Update the position (only when a move was queued)
static_assert(COUNT(target) > 1, "Parameter to _buffer_steps must be (&target)[XYZE]!");
COPY(position, target);
#if HAS_POSITION_FLOAT #if HAS_POSITION_FLOAT
, between_float COPY(position_float, target_float);
#endif #endif
, fr_mm_s, extruder, millimeters * 0.5
);
const uint8_t next = block_buffer_head; // Recalculate and optimize trapezoidal speed profiles
recalculate();
_buffer_steps(target }
#if HAS_POSITION_FLOAT
, target_float
#endif
, fr_mm_s, extruder, millimeters * 0.5
);
SBI(block_buffer[next].flag, BLOCK_BIT_CONTINUED);
ENABLE_STEPPER_DRIVER_INTERRUPT();
} }
else else if (
_buffer_steps(target !_buffer_steps(target
#if HAS_POSITION_FLOAT #if HAS_POSITION_FLOAT
, target_float , target_float
#endif #endif
, fr_mm_s, extruder, millimeters , fr_mm_s, extruder, millimeters
); )
) return false;
stepper.wake_up(); stepper.wake_up();
return true;
} // buffer_segment() } // buffer_segment()
/** /**
@ -2277,7 +2481,7 @@ void Planner::_set_position_mm(const float &a, const float &b, const float &c, c
position_float[C_AXIS] = c; position_float[C_AXIS] = c;
position_float[E_AXIS] = e; position_float[E_AXIS] = e;
#endif #endif
previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest. previous_nominal_speed_sqr = 0.0; // Resets planner junction speeds. Assumes start from rest.
ZERO(previous_speed); ZERO(previous_speed);
buffer_sync_block(); buffer_sync_block();
} }
@ -2297,22 +2501,6 @@ void Planner::set_position_mm_kinematic(const float (&cart)[XYZE]) {
#endif #endif
} }
/**
* Sync from the stepper positions. (e.g., after an interrupted move)
*/
void Planner::sync_from_steppers() {
LOOP_XYZE(i) {
position[i] = stepper.position((AxisEnum)i);
#if HAS_POSITION_FLOAT
position_float[i] = position[i] * steps_to_mm[i
#if ENABLED(DISTINCT_E_FACTORS)
+ (i == E_AXIS ? active_extruder : 0)
#endif
];
#endif
}
}
/** /**
* Setters for planner position (also setting stepper position). * Setters for planner position (also setting stepper position).
*/ */

View file

@ -35,6 +35,7 @@
#include "../Marlin.h" #include "../Marlin.h"
#include "motion.h" #include "motion.h"
#include "../gcode/queue.h"
#if ENABLED(DELTA) #if ENABLED(DELTA)
#include "delta.h" #include "delta.h"
@ -84,19 +85,35 @@ typedef struct {
uint8_t flag; // Block flags (See BlockFlag enum above) uint8_t flag; // Block flags (See BlockFlag enum above)
unsigned char active_extruder; // The extruder to move (if E move) // Fields used by the motion planner to manage acceleration
float nominal_speed_sqr, // The nominal speed for this block in (mm/sec)^2
entry_speed_sqr, // Entry speed at previous-current junction in (mm/sec)^2
max_entry_speed_sqr, // Maximum allowable junction entry speed in (mm/sec)^2
millimeters, // The total travel of this block in mm
acceleration; // acceleration mm/sec^2
// Fields used by the Bresenham algorithm for tracing the line union {
int32_t steps[NUM_AXIS]; // Step count along each axis // Data used by all move blocks
struct {
// Fields used by the Bresenham algorithm for tracing the line
uint32_t steps[NUM_AXIS]; // Step count along each axis
};
// Data used by all sync blocks
struct {
int32_t position[NUM_AXIS]; // New position to force when this sync block is executed
};
};
uint32_t step_event_count; // The number of step events required to complete this block uint32_t step_event_count; // The number of step events required to complete this block
uint8_t active_extruder; // The extruder to move (if E move)
#if ENABLED(MIXING_EXTRUDER) #if ENABLED(MIXING_EXTRUDER)
uint32_t mix_event_count[MIXING_STEPPERS]; // Scaled step_event_count for the mixing steppers uint32_t mix_event_count[MIXING_STEPPERS]; // Scaled step_event_count for the mixing steppers
#endif #endif
// Settings for the trapezoid generator // Settings for the trapezoid generator
int32_t accelerate_until, // The index of the step event on which to stop acceleration uint32_t accelerate_until, // The index of the step event on which to stop acceleration
decelerate_after; // The index of the step event on which to start decelerating decelerate_after; // The index of the step event on which to start decelerating
#if ENABLED(BEZIER_JERK_CONTROL) #if ENABLED(BEZIER_JERK_CONTROL)
uint32_t cruise_rate; // The actual cruise rate to use, between end of the acceleration phase and start of deceleration phase uint32_t cruise_rate; // The actual cruise rate to use, between end of the acceleration phase and start of deceleration phase
@ -105,7 +122,7 @@ typedef struct {
uint32_t acceleration_time_inverse, // Inverse of acceleration and deceleration periods, expressed as integer. Scale depends on CPU being used uint32_t acceleration_time_inverse, // Inverse of acceleration and deceleration periods, expressed as integer. Scale depends on CPU being used
deceleration_time_inverse; deceleration_time_inverse;
#else #else
int32_t acceleration_rate; // The acceleration rate used for acceleration calculation uint32_t acceleration_rate; // The acceleration rate used for acceleration calculation
#endif #endif
uint8_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) uint8_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h)
@ -119,13 +136,6 @@ typedef struct {
float e_D_ratio; float e_D_ratio;
#endif #endif
// Fields used by the motion planner to manage acceleration
float nominal_speed, // The nominal speed for this block in mm/sec
entry_speed, // Entry speed at previous-current junction in mm/sec
max_entry_speed, // Maximum allowable junction entry speed in mm/sec
millimeters, // The total travel of this block in mm
acceleration; // acceleration mm/sec^2
uint32_t nominal_rate, // The nominal step rate for this block in step_events/sec uint32_t nominal_rate, // The nominal step rate for this block in step_events/sec
initial_rate, // The jerk-adjusted step rate at start of block initial_rate, // The jerk-adjusted step rate at start of block
final_rate, // The minimal rate at exit final_rate, // The minimal rate at exit
@ -166,6 +176,7 @@ class Planner {
static block_t block_buffer[BLOCK_BUFFER_SIZE]; static block_t block_buffer[BLOCK_BUFFER_SIZE];
static volatile uint8_t block_buffer_head, // Index of the next block to be pushed static volatile uint8_t block_buffer_head, // Index of the next block to be pushed
block_buffer_tail; // Index of the busy block, if any block_buffer_tail; // Index of the busy block, if any
static int16_t cleaning_buffer_counter; // A counter to disable queuing of blocks
#if ENABLED(DISTINCT_E_FACTORS) #if ENABLED(DISTINCT_E_FACTORS)
static uint8_t last_extruder; // Respond to extruder change static uint8_t last_extruder; // Respond to extruder change
@ -233,6 +244,10 @@ class Planner {
#endif #endif
#endif #endif
#if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
static bool abort_on_endstop_hit;
#endif
private: private:
/** /**
@ -247,9 +262,9 @@ class Planner {
static float previous_speed[NUM_AXIS]; static float previous_speed[NUM_AXIS];
/** /**
* Nominal speed of previous path line segment * Nominal speed of previous path line segment (mm/s)^2
*/ */
static float previous_nominal_speed; static float previous_nominal_speed_sqr;
/** /**
* Limit where 64bit math is necessary for acceleration calculation * Limit where 64bit math is necessary for acceleration calculation
@ -308,15 +323,6 @@ class Planner {
// Manage fans, paste pressure, etc. // Manage fans, paste pressure, etc.
static void check_axes_activity(); static void check_axes_activity();
/**
* Number of moves currently in the planner
*/
FORCE_INLINE static uint8_t movesplanned() { return BLOCK_MOD(block_buffer_head - block_buffer_tail + BLOCK_BUFFER_SIZE); }
FORCE_INLINE static void clear_block_buffer() { block_buffer_head = block_buffer_tail = 0; }
FORCE_INLINE static bool is_full() { return block_buffer_tail == next_block_index(block_buffer_head); }
// Update multipliers based on new diameter measurements // Update multipliers based on new diameter measurements
static void calculate_volumetric_multipliers(); static void calculate_volumetric_multipliers();
@ -424,16 +430,32 @@ class Planner {
#define ARG_Z const float &rz #define ARG_Z const float &rz
#endif #endif
// Number of moves currently in the planner
FORCE_INLINE static uint8_t movesplanned() { return BLOCK_MOD(block_buffer_head - block_buffer_tail); }
// Remove all blocks from the buffer
FORCE_INLINE static void clear_block_buffer() { block_buffer_head = block_buffer_tail = 0; }
// Check if movement queue is full
FORCE_INLINE static bool is_full() { return block_buffer_tail == next_block_index(block_buffer_head); }
// Get count of movement slots free
FORCE_INLINE static uint8_t moves_free() { return BLOCK_BUFFER_SIZE - 1 - movesplanned(); }
/** /**
* Planner::get_next_free_block * Planner::get_next_free_block
* *
* - Get the next head index (passed by reference) * - Get the next head indices (passed by reference)
* - Wait for a space to open up in the planner * - Wait for the number of spaces to open up in the planner
* - Return the head block * - Return the first head block
*/ */
FORCE_INLINE static block_t* get_next_free_block(uint8_t &next_buffer_head) { FORCE_INLINE static block_t* get_next_free_block(uint8_t &next_buffer_head, uint8_t count = 1) {
// Wait until there are enough slots free
while (moves_free() < count) { idle(); }
// Return the first available block
next_buffer_head = next_block_index(block_buffer_head); next_buffer_head = next_block_index(block_buffer_head);
while (block_buffer_tail == next_buffer_head) idle(); // while (is_full)
return &block_buffer[block_buffer_head]; return &block_buffer[block_buffer_head];
} }
@ -446,8 +468,30 @@ class Planner {
* fr_mm_s - (target) speed of the move * fr_mm_s - (target) speed of the move
* extruder - target extruder * extruder - target extruder
* millimeters - the length of the movement, if known * millimeters - the length of the movement, if known
*
* Returns true if movement was buffered, false otherwise
*/ */
static void _buffer_steps(const int32_t (&target)[XYZE] static bool _buffer_steps(const int32_t (&target)[XYZE]
#if HAS_POSITION_FLOAT
, const float (&target_float)[XYZE]
#endif
, float fr_mm_s, const uint8_t extruder, const float &millimeters=0.0
);
/**
* Planner::_populate_block
*
* Fills a new linear movement in the block (in terms of steps).
*
* target - target position in steps units
* fr_mm_s - (target) speed of the move
* extruder - target extruder
* millimeters - the length of the movement, if known
*
* Returns true is movement is acceptable, false otherwise
*/
static bool _populate_block(block_t * const block, bool split_move,
const int32_t (&target)[XYZE]
#if HAS_POSITION_FLOAT #if HAS_POSITION_FLOAT
, const float (&target_float)[XYZE] , const float (&target_float)[XYZE]
#endif #endif
@ -472,7 +516,7 @@ class Planner {
* extruder - target extruder * extruder - target extruder
* millimeters - the length of the movement, if known * millimeters - the length of the movement, if known
*/ */
static void buffer_segment(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder, const float &millimeters=0.0); static bool buffer_segment(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder, const float &millimeters=0.0);
static void _set_position_mm(const float &a, const float &b, const float &c, const float &e); static void _set_position_mm(const float &a, const float &b, const float &c, const float &e);
@ -489,11 +533,11 @@ class Planner {
* extruder - target extruder * extruder - target extruder
* millimeters - the length of the movement, if known * millimeters - the length of the movement, if known
*/ */
FORCE_INLINE static void buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, const float &fr_mm_s, const uint8_t extruder, const float millimeters = 0.0) { FORCE_INLINE static bool buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, const float &fr_mm_s, const uint8_t extruder, const float millimeters = 0.0) {
#if PLANNER_LEVELING && IS_CARTESIAN #if PLANNER_LEVELING && IS_CARTESIAN
apply_leveling(rx, ry, rz); apply_leveling(rx, ry, rz);
#endif #endif
buffer_segment(rx, ry, rz, e, fr_mm_s, extruder, millimeters); return buffer_segment(rx, ry, rz, e, fr_mm_s, extruder, millimeters);
} }
/** /**
@ -506,7 +550,7 @@ class Planner {
* extruder - target extruder * extruder - target extruder
* millimeters - the length of the movement, if known * millimeters - the length of the movement, if known
*/ */
FORCE_INLINE static void buffer_line_kinematic(const float (&cart)[XYZE], const float &fr_mm_s, const uint8_t extruder, const float millimeters = 0.0) { FORCE_INLINE static bool buffer_line_kinematic(const float (&cart)[XYZE], const float &fr_mm_s, const uint8_t extruder, const float millimeters = 0.0) {
#if PLANNER_LEVELING #if PLANNER_LEVELING
float raw[XYZ] = { cart[X_AXIS], cart[Y_AXIS], cart[Z_AXIS] }; float raw[XYZ] = { cart[X_AXIS], cart[Y_AXIS], cart[Z_AXIS] };
apply_leveling(raw); apply_leveling(raw);
@ -515,9 +559,9 @@ class Planner {
#endif #endif
#if IS_KINEMATIC #if IS_KINEMATIC
inverse_kinematics(raw); inverse_kinematics(raw);
buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], cart[E_AXIS], fr_mm_s, extruder, millimeters); return buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], cart[E_AXIS], fr_mm_s, extruder, millimeters);
#else #else
buffer_segment(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], cart[E_AXIS], fr_mm_s, extruder, millimeters); return buffer_segment(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], cart[E_AXIS], fr_mm_s, extruder, millimeters);
#endif #endif
} }
@ -541,11 +585,6 @@ class Planner {
FORCE_INLINE static void set_z_position_mm(const float &z) { set_position_mm(Z_AXIS, z); } FORCE_INLINE static void set_z_position_mm(const float &z) { set_position_mm(Z_AXIS, z); }
FORCE_INLINE static void set_e_position_mm(const float &e) { set_position_mm(E_AXIS, e); } FORCE_INLINE static void set_e_position_mm(const float &e) { set_position_mm(E_AXIS, e); }
/**
* Sync from the stepper positions. (e.g., after an interrupted move)
*/
static void sync_from_steppers();
/** /**
* Get an axis position according to stepper position(s) * Get an axis position according to stepper position(s)
* For CORE machines apply translation from ABC to XYZ. * For CORE machines apply translation from ABC to XYZ.
@ -557,35 +596,38 @@ class Planner {
FORCE_INLINE static float get_axis_position_degrees(const AxisEnum axis) { return get_axis_position_mm(axis); } FORCE_INLINE static float get_axis_position_degrees(const AxisEnum axis) { return get_axis_position_mm(axis); }
#endif #endif
// Called to force a quick stop of the machine (for example, when an emergency
// stop is required, or when endstops are hit)
static void quick_stop();
// Called when an endstop is triggered. Causes the machine to stop inmediately
static void endstop_triggered(const AxisEnum axis);
// Triggered position of an axis in mm (not core-savvy)
static float triggered_position_mm(const AxisEnum axis);
// Block until all buffered steps are executed / cleaned
static void synchronize();
// Wait for moves to finish and disable all steppers
static void finish_and_disable();
// Periodic tick to handle cleaning timeouts
// Called from the Temperature ISR at ~1kHz
static void tick() {
if (cleaning_buffer_counter) {
--cleaning_buffer_counter;
#if ENABLED(SD_FINISHED_STEPPERRELEASE) && defined(SD_FINISHED_RELEASECOMMAND)
if (!cleaning_buffer_counter) enqueue_and_echo_commands_P(PSTR(SD_FINISHED_RELEASECOMMAND));
#endif
}
}
/** /**
* Does the buffer have any blocks queued? * Does the buffer have any blocks queued?
*/ */
FORCE_INLINE static bool has_blocks_queued() { return (block_buffer_head != block_buffer_tail); } FORCE_INLINE static bool has_blocks_queued() { return (block_buffer_head != block_buffer_tail); }
//
// Block until all buffered steps are executed
//
static void synchronize();
/**
* "Discard" the block and "release" the memory.
* Called when the current block is no longer needed.
*/
FORCE_INLINE static void discard_current_block() {
if (has_blocks_queued())
block_buffer_tail = BLOCK_MOD(block_buffer_tail + 1);
}
/**
* "Discard" the next block if it's continued.
* Called after an interrupted move to throw away the rest of the move.
*/
FORCE_INLINE static bool discard_continued_block() {
const bool discard = has_blocks_queued() && TEST(block_buffer[block_buffer_tail].flag, BLOCK_BIT_CONTINUED);
if (discard) discard_current_block();
return discard;
}
/** /**
* The current block. NULL if the buffer is empty. * The current block. NULL if the buffer is empty.
* This also marks the block as busy. * This also marks the block as busy.
@ -618,12 +660,32 @@ class Planner {
} }
} }
/**
* "Discard" the block and "release" the memory.
* Called when the current block is no longer needed.
* NB: There MUST be a current block to call this function!!
*/
FORCE_INLINE static void discard_current_block() {
block_buffer_tail = BLOCK_MOD(block_buffer_tail + 1);
}
#if ENABLED(ULTRA_LCD) #if ENABLED(ULTRA_LCD)
static uint16_t block_buffer_runtime() { static uint16_t block_buffer_runtime() {
CRITICAL_SECTION_START #ifdef __AVR__
millis_t bbru = block_buffer_runtime_us; // Protect the access to the variable. Only required for AVR, as
CRITICAL_SECTION_END // any 32bit CPU offers atomic access to 32bit variables
bool was_enabled = STEPPER_ISR_ENABLED();
if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
#endif
millis_t bbru = block_buffer_runtime_us;
#ifdef __AVR__
// Reenable Stepper ISR
if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
#endif
// To translate µs to ms a division by 1000 would be required. // To translate µs to ms a division by 1000 would be required.
// We introduce 2.4% error here by dividing by 1024. // We introduce 2.4% error here by dividing by 1024.
// Doesn't matter because block_buffer_runtime_us is already too small an estimation. // Doesn't matter because block_buffer_runtime_us is already too small an estimation.
@ -634,9 +696,19 @@ class Planner {
} }
static void clear_block_buffer_runtime() { static void clear_block_buffer_runtime() {
CRITICAL_SECTION_START #ifdef __AVR__
block_buffer_runtime_us = 0; // Protect the access to the variable. Only required for AVR, as
CRITICAL_SECTION_END // any 32bit CPU offers atomic access to 32bit variables
bool was_enabled = STEPPER_ISR_ENABLED();
if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
#endif
block_buffer_runtime_us = 0;
#ifdef __AVR__
// Reenable Stepper ISR
if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
#endif
} }
#endif #endif
@ -679,12 +751,12 @@ class Planner {
} }
/** /**
* Calculate the maximum allowable speed at this point, in order * Calculate the maximum allowable speed squared at this point, in order
* to reach 'target_velocity' using 'acceleration' within a given * to reach 'target_velocity_sqr' using 'acceleration' within a given
* 'distance'. * 'distance'.
*/ */
static float max_allowable_speed(const float &accel, const float &target_velocity, const float &distance) { static float max_allowable_speed_sqr(const float &accel, const float &target_velocity_sqr, const float &distance) {
return SQRT(sq(target_velocity) - 2 * accel * distance); return target_velocity_sqr - 2 * accel * distance;
} }
#if ENABLED(BEZIER_JERK_CONTROL) #if ENABLED(BEZIER_JERK_CONTROL)

View file

@ -194,9 +194,11 @@ void cubic_b_spline(const float position[NUM_AXIS], const float target[NUM_AXIS]
#if HAS_UBL_AND_CURVES #if HAS_UBL_AND_CURVES
float pos[XYZ] = { bez_target[X_AXIS], bez_target[Y_AXIS], bez_target[Z_AXIS] }; float pos[XYZ] = { bez_target[X_AXIS], bez_target[Y_AXIS], bez_target[Z_AXIS] };
planner.apply_leveling(pos); planner.apply_leveling(pos);
planner.buffer_segment(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], bez_target[E_AXIS], fr_mm_s, active_extruder); if (!planner.buffer_segment(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], bez_target[E_AXIS], fr_mm_s, active_extruder))
break;
#else #else
planner.buffer_line_kinematic(bez_target, fr_mm_s, extruder); if (!planner.buffer_line_kinematic(bez_target, fr_mm_s, extruder))
break;
#endif #endif
} }
} }

View file

@ -86,10 +86,6 @@ Stepper stepper; // Singleton
block_t* Stepper::current_block = NULL; // A pointer to the block currently being traced block_t* Stepper::current_block = NULL; // A pointer to the block currently being traced
#if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
bool Stepper::abort_on_endstop_hit = false;
#endif
#if ENABLED(X_DUAL_ENDSTOPS) || ENABLED(Y_DUAL_ENDSTOPS) || ENABLED(Z_DUAL_ENDSTOPS) #if ENABLED(X_DUAL_ENDSTOPS) || ENABLED(Y_DUAL_ENDSTOPS) || ENABLED(Z_DUAL_ENDSTOPS)
bool Stepper::performing_homing = false; bool Stepper::performing_homing = false;
#endif #endif
@ -101,7 +97,6 @@ block_t* Stepper::current_block = NULL; // A pointer to the block currently bei
// private: // private:
uint8_t Stepper::last_direction_bits = 0; // The next stepping-bits to be output uint8_t Stepper::last_direction_bits = 0; // The next stepping-bits to be output
int16_t Stepper::cleaning_buffer_counter = 0;
#if ENABLED(X_DUAL_ENDSTOPS) #if ENABLED(X_DUAL_ENDSTOPS)
bool Stepper::locked_x_motor = false, Stepper::locked_x2_motor = false; bool Stepper::locked_x_motor = false, Stepper::locked_x2_motor = false;
@ -118,7 +113,7 @@ int32_t Stepper::counter_X = 0,
Stepper::counter_Z = 0, Stepper::counter_Z = 0,
Stepper::counter_E = 0; Stepper::counter_E = 0;
volatile uint32_t Stepper::step_events_completed = 0; // The number of step events executed in the current block uint32_t Stepper::step_events_completed = 0; // The number of step events executed in the current block
#if ENABLED(BEZIER_JERK_CONTROL) #if ENABLED(BEZIER_JERK_CONTROL)
int32_t __attribute__((used)) Stepper::bezier_A __asm__("bezier_A"); // A coefficient in Bézier speed curve with alias for assembler int32_t __attribute__((used)) Stepper::bezier_A __asm__("bezier_A"); // A coefficient in Bézier speed curve with alias for assembler
@ -132,15 +127,16 @@ volatile uint32_t Stepper::step_events_completed = 0; // The number of step even
bool Stepper::bezier_2nd_half; // =false If Bézier curve has been initialized or not bool Stepper::bezier_2nd_half; // =false If Bézier curve has been initialized or not
#endif #endif
uint32_t Stepper::nextMainISR = 0;
bool Stepper::all_steps_done = false;
#if ENABLED(LIN_ADVANCE) #if ENABLED(LIN_ADVANCE)
uint32_t Stepper::LA_decelerate_after; uint32_t Stepper::LA_decelerate_after;
constexpr hal_timer_t ADV_NEVER = HAL_TIMER_TYPE_MAX; constexpr uint32_t ADV_NEVER = 0xFFFFFFFF;
uint32_t Stepper::nextAdvanceISR = ADV_NEVER,
hal_timer_t Stepper::nextMainISR = 0, Stepper::eISR_Rate = ADV_NEVER;
Stepper::nextAdvanceISR = ADV_NEVER,
Stepper::eISR_Rate = ADV_NEVER;
uint16_t Stepper::current_adv_steps = 0, uint16_t Stepper::current_adv_steps = 0,
Stepper::final_adv_steps, Stepper::final_adv_steps,
Stepper::max_adv_steps; Stepper::max_adv_steps;
@ -157,7 +153,7 @@ volatile uint32_t Stepper::step_events_completed = 0; // The number of step even
#endif // LIN_ADVANCE #endif // LIN_ADVANCE
int32_t Stepper::acceleration_time, Stepper::deceleration_time; uint32_t Stepper::acceleration_time, Stepper::deceleration_time;
volatile int32_t Stepper::count_position[NUM_AXIS] = { 0 }; volatile int32_t Stepper::count_position[NUM_AXIS] = { 0 };
volatile signed char Stepper::count_direction[NUM_AXIS] = { 1, 1, 1, 1 }; volatile signed char Stepper::count_direction[NUM_AXIS] = { 1, 1, 1, 1 };
@ -166,11 +162,11 @@ volatile signed char Stepper::count_direction[NUM_AXIS] = { 1, 1, 1, 1 };
int32_t Stepper::counter_m[MIXING_STEPPERS]; int32_t Stepper::counter_m[MIXING_STEPPERS];
#endif #endif
uint32_t Stepper::ticks_nominal;
uint8_t Stepper::step_loops, Stepper::step_loops_nominal; uint8_t Stepper::step_loops, Stepper::step_loops_nominal;
hal_timer_t Stepper::OCR1A_nominal;
#if DISABLED(BEZIER_JERK_CONTROL) #if DISABLED(BEZIER_JERK_CONTROL)
hal_timer_t Stepper::acc_step_rate; // needed for deceleration start point uint32_t Stepper::acc_step_rate; // needed for deceleration start point
#endif #endif
volatile int32_t Stepper::endstops_trigsteps[XYZ]; volatile int32_t Stepper::endstops_trigsteps[XYZ];
@ -379,7 +375,7 @@ void Stepper::set_directions() {
* *
* Floating point arithmetic execution time cost is prohibitive, so we will transform the math to * Floating point arithmetic execution time cost is prohibitive, so we will transform the math to
* use fixed point values to be able to evaluate it in realtime. Assuming a maximum of 250000 steps * use fixed point values to be able to evaluate it in realtime. Assuming a maximum of 250000 steps
* per second (driver pulses should at least be 2uS hi/2uS lo), and allocating 2 bits to avoid * per second (driver pulses should at least be 2µS hi/2µS lo), and allocating 2 bits to avoid
* overflows on the evaluation of the Bézier curve, means we can use * overflows on the evaluation of the Bézier curve, means we can use
* *
* t: unsigned Q0.32 (0 <= t < 1) |range 0 to 0xFFFFFFFF unsigned * t: unsigned Q0.32 (0 <= t < 1) |range 0 to 0xFFFFFFFF unsigned
@ -1149,11 +1145,27 @@ void Stepper::set_directions() {
HAL_STEP_TIMER_ISR { HAL_STEP_TIMER_ISR {
HAL_timer_isr_prologue(STEP_TIMER_NUM); HAL_timer_isr_prologue(STEP_TIMER_NUM);
#if ENABLED(LIN_ADVANCE) // Program timer compare for the maximum period, so it does NOT
Stepper::advance_isr_scheduler(); // flag an interrupt while this ISR is running - So changes from small
#else // periods to big periods are respected and the timer does not reset to 0
Stepper::isr(); HAL_timer_set_compare(STEP_TIMER_NUM, HAL_TIMER_TYPE_MAX);
#endif
// Call the ISR scheduler
hal_timer_t ticks = Stepper::isr_scheduler();
// Now 'ticks' contains the period to the next Stepper ISR.
// Potential problem: Since the timer continues to run, the requested
// compare value may already have passed.
//
// Assuming at least 6µs between calls to this ISR...
// On AVR the ISR epilogue is estimated at 40 instructions - close to 2.5µS.
// On ARM the ISR epilogue is estimated at 10 instructions - close to 200nS.
// In either case leave at least 4µS for other tasks to execute.
const hal_timer_t minticks = HAL_timer_get_count(STEP_TIMER_NUM) + hal_timer_t((HAL_TICKS_PER_US) * 4); // ISR never takes more than 1ms, so this shouldn't cause trouble
NOLESS(ticks, MAX(minticks, hal_timer_t((STEP_TIMER_MIN_INTERVAL) * (HAL_TICKS_PER_US))));
// Set the next ISR to fire at the proper time
HAL_timer_set_compare(STEP_TIMER_NUM, ticks);
HAL_timer_isr_epilogue(STEP_TIMER_NUM); HAL_timer_isr_epilogue(STEP_TIMER_NUM);
} }
@ -1164,168 +1176,64 @@ HAL_STEP_TIMER_ISR {
#define STEP_MULTIPLY(A,B) MultiU24X32toH16(A, B) #define STEP_MULTIPLY(A,B) MultiU24X32toH16(A, B)
#endif #endif
void Stepper::isr() { hal_timer_t Stepper::isr_scheduler() {
uint32_t interval;
#define ENDSTOP_NOMINAL_OCR_VAL 1500 * HAL_TICKS_PER_US // Check endstops every 1.5ms to guarantee two stepper ISRs within 5ms for BLTouch // Run main stepping pulse phase ISR if we have to
#define OCR_VAL_TOLERANCE 500 * HAL_TICKS_PER_US // First max delay is 2.0ms, last min delay is 0.5ms, all others 1.5ms if (!nextMainISR) Stepper::stepper_pulse_phase_isr();
hal_timer_t ocr_val; #if ENABLED(LIN_ADVANCE)
static uint32_t step_remaining = 0; // SPLIT function always runs. This allows 16 bit timers to be // Run linear advance stepper ISR if we have to
// used to generate the stepper ISR. if (!nextAdvanceISR) nextAdvanceISR = Stepper::advance_isr();
#define SPLIT(L) do { \
if (L > ENDSTOP_NOMINAL_OCR_VAL) { \
const uint32_t remainder = (uint32_t)L % (ENDSTOP_NOMINAL_OCR_VAL); \
ocr_val = (remainder < OCR_VAL_TOLERANCE) ? ENDSTOP_NOMINAL_OCR_VAL + remainder : ENDSTOP_NOMINAL_OCR_VAL; \
step_remaining = (uint32_t)L - ocr_val; \
} \
else \
ocr_val = L;\
}while(0)
// Time remaining before the next step?
if (step_remaining) {
// Make sure endstops are updated
if (ENDSTOPS_ENABLED) endstops.update();
// Next ISR either for endstops or stepping
ocr_val = step_remaining <= ENDSTOP_NOMINAL_OCR_VAL ? step_remaining : ENDSTOP_NOMINAL_OCR_VAL;
step_remaining -= ocr_val;
_NEXT_ISR(ocr_val);
#if DISABLED(LIN_ADVANCE)
HAL_timer_restrain(STEP_TIMER_NUM, STEP_TIMER_MIN_INTERVAL * HAL_TICKS_PER_US);
#endif
return;
}
//
// When cleaning, discard the current block and run fast
//
if (cleaning_buffer_counter) {
if (cleaning_buffer_counter < 0) { // Count up for endstop hit
if (current_block) planner.discard_current_block(); // Discard the active block that led to the trigger
if (!planner.discard_continued_block()) // Discard next CONTINUED block
cleaning_buffer_counter = 0; // Keep discarding until non-CONTINUED
}
else {
planner.discard_current_block();
--cleaning_buffer_counter; // Count down for abort print
#if ENABLED(SD_FINISHED_STEPPERRELEASE) && defined(SD_FINISHED_RELEASECOMMAND)
if (!cleaning_buffer_counter) enqueue_and_echo_commands_P(PSTR(SD_FINISHED_RELEASECOMMAND));
#endif
}
current_block = NULL; // Prep to get a new block after cleaning
_NEXT_ISR(HAL_STEPPER_TIMER_RATE / 10000); // Run at max speed - 10 KHz
return;
}
// If there is no current block, attempt to pop one from the buffer
if (!current_block) {
// Anything in the buffer?
if ((current_block = planner.get_current_block())) {
// Sync block? Sync the stepper counts and return
while (TEST(current_block->flag, BLOCK_BIT_SYNC_POSITION)) {
_set_position(
current_block->steps[A_AXIS], current_block->steps[B_AXIS],
current_block->steps[C_AXIS], current_block->steps[E_AXIS]
);
planner.discard_current_block();
if (!(current_block = planner.get_current_block())) return;
}
// Initialize the trapezoid generator from the current block.
static int8_t last_extruder = -1;
#if ENABLED(LIN_ADVANCE)
#if E_STEPPERS > 1
if (current_block->active_extruder != last_extruder) {
current_adv_steps = 0; // If the now active extruder wasn't in use during the last move, its pressure is most likely gone.
LA_active_extruder = current_block->active_extruder;
}
#endif
if ((use_advance_lead = current_block->use_advance_lead)) {
LA_decelerate_after = current_block->decelerate_after;
final_adv_steps = current_block->final_adv_steps;
max_adv_steps = current_block->max_adv_steps;
}
#endif
if (current_block->direction_bits != last_direction_bits || current_block->active_extruder != last_extruder) {
last_direction_bits = current_block->direction_bits;
last_extruder = current_block->active_extruder;
set_directions();
}
// No acceleration / deceleration time elapsed so far
acceleration_time = deceleration_time = 0;
// No step events completed so far
step_events_completed = 0;
// step_rate to timer interval
OCR1A_nominal = calc_timer_interval(current_block->nominal_rate);
// make a note of the number of step loops required at nominal speed
step_loops_nominal = step_loops;
#if DISABLED(BEZIER_JERK_CONTROL)
// Set as deceleration point the initial rate of the block
acc_step_rate = current_block->initial_rate;
#endif
#if ENABLED(BEZIER_JERK_CONTROL)
// Initialize the Bézier speed curve
_calc_bezier_curve_coeffs(current_block->initial_rate, current_block->cruise_rate, current_block->acceleration_time_inverse);
// We have not started the 2nd half of the trapezoid
bezier_2nd_half = false;
#endif
// Initialize Bresenham counters to 1/2 the ceiling
counter_X = counter_Y = counter_Z = counter_E = -(current_block->step_event_count >> 1);
#if ENABLED(MIXING_EXTRUDER)
MIXING_STEPPERS_LOOP(i)
counter_m[i] = -(current_block->mix_event_count[i] >> 1);
#endif
#if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
e_hit = 2; // Needed for the case an endstop is already triggered before the new move begins.
// No 'change' can be detected.
#endif
#if ENABLED(Z_LATE_ENABLE)
// If delayed Z enable, postpone move for 1mS
if (current_block->steps[Z_AXIS] > 0) {
enable_Z();
_NEXT_ISR(HAL_STEPPER_TIMER_RATE / 1000); // Run at slow speed - 1 KHz
return;
}
#endif
}
else {
// If no more queued moves, postpone next check for 1mS
_NEXT_ISR(HAL_STEPPER_TIMER_RATE / 1000); // Run at slow speed - 1 KHz
return;
}
}
// Update endstops state, if enabled
#if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
if (e_hit && ENDSTOPS_ENABLED) {
endstops.update();
e_hit--;
}
#else
if (ENDSTOPS_ENABLED) endstops.update();
#endif #endif
// ^== Time critical. NOTHING besides pulse generation should be above here!!!
// Run main stepping block processing ISR if we have to
if (!nextMainISR) nextMainISR = Stepper::stepper_block_phase_isr();
#if ENABLED(LIN_ADVANCE)
// Select the closest interval in time
interval = (nextAdvanceISR <= nextMainISR)
? nextAdvanceISR
: nextMainISR;
#else // !ENABLED(LIN_ADVANCE)
// The interval is just the remaining time to the stepper ISR
interval = nextMainISR;
#endif
// Limit the value to the maximum possible value of the timer
if (interval > HAL_TIMER_TYPE_MAX)
interval = HAL_TIMER_TYPE_MAX;
// Compute the time remaining for the main isr
nextMainISR -= interval;
#if ENABLED(LIN_ADVANCE)
// Compute the time remaining for the advance isr
if (nextAdvanceISR != ADV_NEVER)
nextAdvanceISR -= interval;
#endif
return (hal_timer_t)interval;
}
// This part of the ISR should ONLY create the pulses for the steppers
// -- Nothing more, nothing less -- We want to avoid jitter from where
// the pulses should be generated (when the interrupt triggers) to the
// time pulses are actually created. So, PLEASE DO NOT PLACE ANY CODE
// above this line that can conditionally change that time (we are trying
// to keep the delay between the interrupt triggering and pulse generation
// as constant as possible!!!!
void Stepper::stepper_pulse_phase_isr() {
// If there is no current block, do nothing
if (!current_block) return;
// Take multiple steps per interrupt (For high speed moves) // Take multiple steps per interrupt (For high speed moves)
bool all_steps_done = false; all_steps_done = false;
for (uint8_t i = step_loops; i--;) { for (uint8_t i = step_loops; i--;) {
#define _COUNTER(AXIS) counter_## AXIS #define _COUNTER(AXIS) counter_## AXIS
@ -1520,116 +1428,208 @@ void Stepper::isr() {
#endif #endif
} // steps_loop } // steps_loop
}
// Calculate new timer value // This is the last half of the stepper interrupt: This one processes and
if (step_events_completed <= (uint32_t)current_block->accelerate_until) { // properly schedules blocks from the planner. This is executed after creating
// the step pulses, so it is not time critical, as pulses are already done.
#if ENABLED(BEZIER_JERK_CONTROL) uint32_t Stepper::stepper_block_phase_isr() {
// Get the next speed to use (Jerk limited!)
hal_timer_t acc_step_rate =
acceleration_time < current_block->acceleration_time
? _eval_bezier_curve(acceleration_time)
: current_block->cruise_rate;
#else
acc_step_rate = STEP_MULTIPLY(acceleration_time, current_block->acceleration_rate) + current_block->initial_rate;
NOMORE(acc_step_rate, current_block->nominal_rate);
#endif
// step_rate to timer interval // If no queued movements, just wait 1ms for the next move
const hal_timer_t interval = calc_timer_interval(acc_step_rate); uint32_t interval = (HAL_STEPPER_TIMER_RATE / 1000);
SPLIT(interval); // split step into multiple ISRs if larger than ENDSTOP_NOMINAL_OCR_VAL // If there is a current block
_NEXT_ISR(ocr_val); if (current_block) {
acceleration_time += interval; // Calculate new timer value
if (step_events_completed <= current_block->accelerate_until) {
#if ENABLED(LIN_ADVANCE) #if ENABLED(BEZIER_JERK_CONTROL)
if (current_block->use_advance_lead) { // Get the next speed to use (Jerk limited!)
if (step_events_completed == step_loops || (e_steps && eISR_Rate != current_block->advance_speed)) { uint32_t acc_step_rate =
nextAdvanceISR = 0; // Wake up eISR on first acceleration loop and fire ISR if final adv_rate is reached acceleration_time < current_block->acceleration_time
eISR_Rate = current_block->advance_speed; ? _eval_bezier_curve(acceleration_time)
: current_block->cruise_rate;
#else
acc_step_rate = STEP_MULTIPLY(acceleration_time, current_block->acceleration_rate) + current_block->initial_rate;
NOMORE(acc_step_rate, current_block->nominal_rate);
#endif
// step_rate to timer interval
interval = calc_timer_interval(acc_step_rate);
acceleration_time += interval;
#if ENABLED(LIN_ADVANCE)
if (current_block->use_advance_lead) {
if (step_events_completed == step_loops || (e_steps && eISR_Rate != current_block->advance_speed)) {
nextAdvanceISR = 0; // Wake up eISR on first acceleration loop and fire ISR if final adv_rate is reached
eISR_Rate = current_block->advance_speed;
}
} }
} else {
else { eISR_Rate = ADV_NEVER;
eISR_Rate = ADV_NEVER; if (e_steps) nextAdvanceISR = 0;
if (e_steps) nextAdvanceISR = 0; }
} #endif // LIN_ADVANCE
#endif // LIN_ADVANCE }
else if (step_events_completed > current_block->decelerate_after) {
uint32_t step_rate;
#if ENABLED(BEZIER_JERK_CONTROL)
// If this is the 1st time we process the 2nd half of the trapezoid...
if (!bezier_2nd_half) {
// Initialize the Bézier speed curve
_calc_bezier_curve_coeffs(current_block->cruise_rate, current_block->final_rate, current_block->deceleration_time_inverse);
bezier_2nd_half = true;
}
// Calculate the next speed to use
step_rate = deceleration_time < current_block->deceleration_time
? _eval_bezier_curve(deceleration_time)
: current_block->final_rate;
#else
// Using the old trapezoidal control
step_rate = STEP_MULTIPLY(deceleration_time, current_block->acceleration_rate);
if (step_rate < acc_step_rate) { // Still decelerating?
step_rate = acc_step_rate - step_rate;
NOLESS(step_rate, current_block->final_rate);
}
else
step_rate = current_block->final_rate;
#endif
// step_rate to timer interval
interval = calc_timer_interval(step_rate);
deceleration_time += interval;
#if ENABLED(LIN_ADVANCE)
if (current_block->use_advance_lead) {
if (step_events_completed <= current_block->decelerate_after + step_loops || (e_steps && eISR_Rate != current_block->advance_speed)) {
nextAdvanceISR = 0; // Wake up eISR on first deceleration loop
eISR_Rate = current_block->advance_speed;
}
}
else {
eISR_Rate = ADV_NEVER;
if (e_steps) nextAdvanceISR = 0;
}
#endif // LIN_ADVANCE
}
else {
#if ENABLED(LIN_ADVANCE)
// If there are any esteps, fire the next advance_isr "now"
if (e_steps && eISR_Rate != current_block->advance_speed) nextAdvanceISR = 0;
#endif
// The timer interval is just the nominal value for the nominal speed
interval = ticks_nominal;
// Ensure this runs at the correct step rate, even if it just came off an acceleration
step_loops = step_loops_nominal;
}
// If current block is finished, reset pointer
if (all_steps_done) {
current_block = NULL;
planner.discard_current_block();
}
} }
else if (step_events_completed > (uint32_t)current_block->decelerate_after) {
hal_timer_t step_rate;
#if ENABLED(BEZIER_JERK_CONTROL) // If there is no current block at this point, attempt to pop one from the buffer
// If this is the 1st time we process the 2nd half of the trapezoid... // and prepare its movement
if (!bezier_2nd_half) { if (!current_block) {
// Anything in the buffer?
if ((current_block = planner.get_current_block())) {
// Sync block? Sync the stepper counts and return
while (TEST(current_block->flag, BLOCK_BIT_SYNC_POSITION)) {
_set_position(
current_block->position[A_AXIS], current_block->position[B_AXIS],
current_block->position[C_AXIS], current_block->position[E_AXIS]
);
planner.discard_current_block();
// Try to get a new block
if (!(current_block = planner.get_current_block()))
return interval; // No more queued movements!
}
// Initialize the trapezoid generator from the current block.
static int8_t last_extruder = -1;
#if ENABLED(LIN_ADVANCE)
#if E_STEPPERS > 1
if (current_block->active_extruder != last_extruder) {
current_adv_steps = 0; // If the now active extruder wasn't in use during the last move, its pressure is most likely gone.
LA_active_extruder = current_block->active_extruder;
}
#endif
if ((use_advance_lead = current_block->use_advance_lead)) {
LA_decelerate_after = current_block->decelerate_after;
final_adv_steps = current_block->final_adv_steps;
max_adv_steps = current_block->max_adv_steps;
}
#endif
if (current_block->direction_bits != last_direction_bits || current_block->active_extruder != last_extruder) {
last_direction_bits = current_block->direction_bits;
last_extruder = current_block->active_extruder;
set_directions();
}
// No acceleration / deceleration time elapsed so far
acceleration_time = deceleration_time = 0;
// No step events completed so far
step_events_completed = 0;
// step_rate to timer interval for the nominal speed
ticks_nominal = calc_timer_interval(current_block->nominal_rate);
// make a note of the number of step loops required at nominal speed
step_loops_nominal = step_loops;
#if DISABLED(BEZIER_JERK_CONTROL)
// Set as deceleration point the initial rate of the block
acc_step_rate = current_block->initial_rate;
#endif
#if ENABLED(BEZIER_JERK_CONTROL)
// Initialize the Bézier speed curve // Initialize the Bézier speed curve
_calc_bezier_curve_coeffs(current_block->cruise_rate, current_block->final_rate, current_block->deceleration_time_inverse); _calc_bezier_curve_coeffs(current_block->initial_rate, current_block->cruise_rate, current_block->acceleration_time_inverse);
bezier_2nd_half = true;
}
// Calculate the next speed to use // We have not started the 2nd half of the trapezoid
step_rate = deceleration_time < current_block->deceleration_time bezier_2nd_half = false;
? _eval_bezier_curve(deceleration_time) #endif
: current_block->final_rate;
#else
// Using the old trapezoidal control // Initialize Bresenham counters to 1/2 the ceiling
step_rate = STEP_MULTIPLY(deceleration_time, current_block->acceleration_rate); counter_X = counter_Y = counter_Z = counter_E = -((int32_t)(current_block->step_event_count >> 1));
if (step_rate < acc_step_rate) { // Still decelerating? #if ENABLED(MIXING_EXTRUDER)
step_rate = acc_step_rate - step_rate; MIXING_STEPPERS_LOOP(i)
NOLESS(step_rate, current_block->final_rate); counter_m[i] = -(current_block->mix_event_count[i] >> 1);
} #endif
else
step_rate = current_block->final_rate;
#endif #if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
e_hit = 2; // Needed for the case an endstop is already triggered before the new move begins.
// No 'change' can be detected.
#endif
// step_rate to timer interval #if ENABLED(Z_LATE_ENABLE)
const hal_timer_t interval = calc_timer_interval(step_rate); // If delayed Z enable, enable it now. This option will severely interfere with
// timing between pulses when chaining motion between blocks, and it could lead
SPLIT(interval); // split step into multiple ISRs if larger than ENDSTOP_NOMINAL_OCR_VAL // to lost steps in both X and Y axis, so avoid using it unless strictly necessary!!
_NEXT_ISR(ocr_val); if (current_block->steps[Z_AXIS]) enable_Z();
#endif
deceleration_time += interval; }
#if ENABLED(LIN_ADVANCE)
if (current_block->use_advance_lead) {
if (step_events_completed <= (uint32_t)current_block->decelerate_after + step_loops || (e_steps && eISR_Rate != current_block->advance_speed)) {
nextAdvanceISR = 0; // Wake up eISR on first deceleration loop
eISR_Rate = current_block->advance_speed;
}
}
else {
eISR_Rate = ADV_NEVER;
if (e_steps) nextAdvanceISR = 0;
}
#endif // LIN_ADVANCE
}
else {
#if ENABLED(LIN_ADVANCE)
// If we have esteps to execute, fire the next advance_isr "now"
if (e_steps && eISR_Rate != current_block->advance_speed) nextAdvanceISR = 0;
#endif
SPLIT(OCR1A_nominal); // split step into multiple ISRs if larger than ENDSTOP_NOMINAL_OCR_VAL
_NEXT_ISR(ocr_val);
// ensure we're running at the correct step rate, even if we just came off an acceleration
step_loops = step_loops_nominal;
} }
#if DISABLED(LIN_ADVANCE) // Return the interval to wait
// Make sure stepper ISR doesn't monopolize the CPU return interval;
HAL_timer_restrain(STEP_TIMER_NUM, STEP_TIMER_MIN_INTERVAL * HAL_TICKS_PER_US);
#endif
// If current block is finished, reset pointer
if (all_steps_done) {
current_block = NULL;
planner.discard_current_block();
}
} }
#if ENABLED(LIN_ADVANCE) #if ENABLED(LIN_ADVANCE)
@ -1638,8 +1638,8 @@ void Stepper::isr() {
#define EXTRA_CYCLES_E (STEP_PULSE_CYCLES - (CYCLES_EATEN_E)) #define EXTRA_CYCLES_E (STEP_PULSE_CYCLES - (CYCLES_EATEN_E))
// Timer interrupt for E. e_steps is set in the main routine; // Timer interrupt for E. e_steps is set in the main routine;
uint32_t Stepper::advance_isr() {
void Stepper::advance_isr() { uint32_t interval;
#if ENABLED(MK2_MULTIPLEXER) // For SNMM even-numbered steppers are reversed #if ENABLED(MK2_MULTIPLEXER) // For SNMM even-numbered steppers are reversed
#define SET_E_STEP_DIR(INDEX) do{ if (e_steps) E0_DIR_WRITE(e_steps < 0 ? !INVERT_E## INDEX ##_DIR ^ TEST(INDEX, 0) : INVERT_E## INDEX ##_DIR ^ TEST(INDEX, 0)); }while(0) #define SET_E_STEP_DIR(INDEX) do{ if (e_steps) E0_DIR_WRITE(e_steps < 0 ? !INVERT_E## INDEX ##_DIR ^ TEST(INDEX, 0) : INVERT_E## INDEX ##_DIR ^ TEST(INDEX, 0)); }while(0)
@ -1700,21 +1700,21 @@ void Stepper::isr() {
if (step_events_completed > LA_decelerate_after && current_adv_steps > final_adv_steps) { if (step_events_completed > LA_decelerate_after && current_adv_steps > final_adv_steps) {
e_steps--; e_steps--;
current_adv_steps--; current_adv_steps--;
nextAdvanceISR = eISR_Rate; interval = eISR_Rate;
} }
else if (step_events_completed < LA_decelerate_after && current_adv_steps < max_adv_steps) { else if (step_events_completed < LA_decelerate_after && current_adv_steps < max_adv_steps) {
//step_events_completed <= (uint32_t)current_block->accelerate_until) { //step_events_completed <= (uint32_t)current_block->accelerate_until) {
e_steps++; e_steps++;
current_adv_steps++; current_adv_steps++;
nextAdvanceISR = eISR_Rate; interval = eISR_Rate;
} }
else { else {
nextAdvanceISR = ADV_NEVER; interval = ADV_NEVER;
eISR_Rate = ADV_NEVER; eISR_Rate = ADV_NEVER;
} }
} }
else else
nextAdvanceISR = ADV_NEVER; interval = ADV_NEVER;
switch (LA_active_extruder) { switch (LA_active_extruder) {
case 0: SET_E_STEP_DIR(0); break; case 0: SET_E_STEP_DIR(0); break;
@ -1787,39 +1787,9 @@ void Stepper::isr() {
#endif #endif
} // e_steps } // e_steps
return interval;
} }
void Stepper::advance_isr_scheduler() {
// Run main stepping ISR if flagged
if (!nextMainISR) isr();
// Run Advance stepping ISR if flagged
if (!nextAdvanceISR) advance_isr();
// Is the next advance ISR scheduled before the next main ISR?
if (nextAdvanceISR <= nextMainISR) {
// Set up the next interrupt
HAL_timer_set_compare(STEP_TIMER_NUM, nextAdvanceISR);
// New interval for the next main ISR
if (nextMainISR) nextMainISR -= nextAdvanceISR;
// Will call Stepper::advance_isr on the next interrupt
nextAdvanceISR = 0;
}
else {
// The next main ISR comes first
HAL_timer_set_compare(STEP_TIMER_NUM, nextMainISR);
// New interval for the next advance ISR, if any
if (nextAdvanceISR && nextAdvanceISR != ADV_NEVER)
nextAdvanceISR -= nextMainISR;
// Will call Stepper::isr on the next interrupt
nextMainISR = 0;
}
// Make sure stepper ISR doesn't monopolize the CPU
HAL_timer_restrain(STEP_TIMER_NUM, STEP_TIMER_MIN_INTERVAL * HAL_TICKS_PER_US);
}
#endif // LIN_ADVANCE #endif // LIN_ADVANCE
void Stepper::init() { void Stepper::init() {
@ -2048,30 +2018,47 @@ void Stepper::_set_position(const int32_t &a, const int32_t &b, const int32_t &c
* Get a stepper's position in steps. * Get a stepper's position in steps.
*/ */
int32_t Stepper::position(const AxisEnum axis) { int32_t Stepper::position(const AxisEnum axis) {
CRITICAL_SECTION_START; #ifdef __AVR__
const int32_t count_pos = count_position[axis]; // Protect the access to the position. Only required for AVR, as
CRITICAL_SECTION_END; // any 32bit CPU offers atomic access to 32bit variables
return count_pos; const bool was_enabled = STEPPER_ISR_ENABLED();
} if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
#endif
void Stepper::finish_and_disable() { int32_t v = count_position[axis];
planner.synchronize();
disable_all_steppers(); #ifdef __AVR__
// Reenable Stepper ISR
if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
#endif
return v;
} }
void Stepper::quick_stop() { void Stepper::quick_stop() {
const bool was_enabled = STEPPER_ISR_ENABLED();
DISABLE_STEPPER_DRIVER_INTERRUPT(); DISABLE_STEPPER_DRIVER_INTERRUPT();
kill_current_block();
current_block = NULL; if (current_block) {
cleaning_buffer_counter = 5000; step_events_completed = current_block->step_event_count;
planner.clear_block_buffer(); current_block = NULL;
ENABLE_STEPPER_DRIVER_INTERRUPT(); }
#if ENABLED(ULTRA_LCD)
planner.clear_block_buffer_runtime(); if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
#endif }
void Stepper::kill_current_block() {
const bool was_enabled = STEPPER_ISR_ENABLED();
DISABLE_STEPPER_DRIVER_INTERRUPT();
if (current_block)
step_events_completed = current_block->step_event_count;
if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
} }
void Stepper::endstop_triggered(const AxisEnum axis) { void Stepper::endstop_triggered(const AxisEnum axis) {
const bool was_enabled = STEPPER_ISR_ENABLED();
if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
#if IS_CORE #if IS_CORE
@ -2086,16 +2073,48 @@ void Stepper::endstop_triggered(const AxisEnum axis) {
#endif // !COREXY && !COREXZ && !COREYZ #endif // !COREXY && !COREXZ && !COREYZ
kill_current_block(); // Discard the rest of the move if there is a current block
cleaning_buffer_counter = -1; // Discard the rest of the move if (current_block) {
// Kill the current block being executed
step_events_completed = current_block->step_event_count;
// Prep to get a new block after cleaning
current_block = NULL;
}
if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
}
int32_t Stepper::triggered_position(const AxisEnum axis) {
#ifdef __AVR__
// Protect the access to the position. Only required for AVR, as
// any 32bit CPU offers atomic access to 32bit variables
const bool was_enabled = STEPPER_ISR_ENABLED();
if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
#endif
const int32_t v = endstops_trigsteps[axis];
#ifdef __AVR__
// Reenable Stepper ISR
if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
#endif
return v;
} }
void Stepper::report_positions() { void Stepper::report_positions() {
CRITICAL_SECTION_START;
// Protect the access to the position.
const bool was_enabled = STEPPER_ISR_ENABLED();
if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
const int32_t xpos = count_position[X_AXIS], const int32_t xpos = count_position[X_AXIS],
ypos = count_position[Y_AXIS], ypos = count_position[Y_AXIS],
zpos = count_position[Z_AXIS]; zpos = count_position[Z_AXIS];
CRITICAL_SECTION_END;
if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
#if CORE_IS_XY || CORE_IS_XZ || IS_DELTA || IS_SCARA #if CORE_IS_XY || CORE_IS_XZ || IS_DELTA || IS_SCARA
SERIAL_PROTOCOLPGM(MSG_COUNT_A); SERIAL_PROTOCOLPGM(MSG_COUNT_A);

View file

@ -62,10 +62,6 @@ class Stepper {
static block_t* current_block; // A pointer to the block currently being traced static block_t* current_block; // A pointer to the block currently being traced
#if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
static bool abort_on_endstop_hit;
#endif
#if ENABLED(X_DUAL_ENDSTOPS) || ENABLED(Y_DUAL_ENDSTOPS) || ENABLED(Z_DUAL_ENDSTOPS) #if ENABLED(X_DUAL_ENDSTOPS) || ENABLED(Y_DUAL_ENDSTOPS) || ENABLED(Z_DUAL_ENDSTOPS)
static bool performing_homing; static bool performing_homing;
#endif #endif
@ -77,8 +73,6 @@ class Stepper {
static uint32_t motor_current_setting[3]; static uint32_t motor_current_setting[3];
#endif #endif
static int16_t cleaning_buffer_counter;
private: private:
static uint8_t last_direction_bits; // The next stepping-bits to be output static uint8_t last_direction_bits; // The next stepping-bits to be output
@ -95,7 +89,7 @@ class Stepper {
// Counter variables for the Bresenham line tracer // Counter variables for the Bresenham line tracer
static int32_t counter_X, counter_Y, counter_Z, counter_E; static int32_t counter_X, counter_Y, counter_Z, counter_E;
static volatile uint32_t step_events_completed; // The number of step events executed in the current block static uint32_t step_events_completed; // The number of step events executed in the current block
#if ENABLED(BEZIER_JERK_CONTROL) #if ENABLED(BEZIER_JERK_CONTROL)
static int32_t bezier_A, // A coefficient in Bézier speed curve static int32_t bezier_A, // A coefficient in Bézier speed curve
@ -109,12 +103,14 @@ class Stepper {
static bool bezier_2nd_half; // If Bézier curve has been initialized or not static bool bezier_2nd_half; // If Bézier curve has been initialized or not
#endif #endif
static uint32_t nextMainISR; // time remaining for the next Step ISR
static bool all_steps_done; // all steps done
#if ENABLED(LIN_ADVANCE) #if ENABLED(LIN_ADVANCE)
static uint32_t LA_decelerate_after; // Copy from current executed block. Needed because current_block is set to NULL "too early". static uint32_t LA_decelerate_after; // Copy from current executed block. Needed because current_block is set to NULL "too early".
static hal_timer_t nextMainISR, nextAdvanceISR, eISR_Rate; static uint32_t nextAdvanceISR, eISR_Rate;
static uint16_t current_adv_steps, final_adv_steps, max_adv_steps; // Copy from current executed block. Needed because current_block is set to NULL "too early". static uint16_t current_adv_steps, final_adv_steps, max_adv_steps; // Copy from current executed block. Needed because current_block is set to NULL "too early".
#define _NEXT_ISR(T) nextMainISR = T
static int8_t e_steps; static int8_t e_steps;
static bool use_advance_lead; static bool use_advance_lead;
#if E_STEPPERS > 1 #if E_STEPPERS > 1
@ -123,18 +119,14 @@ class Stepper {
static constexpr int8_t LA_active_extruder = 0; static constexpr int8_t LA_active_extruder = 0;
#endif #endif
#else // !LIN_ADVANCE #endif // LIN_ADVANCE
#define _NEXT_ISR(T) HAL_timer_set_compare(STEP_TIMER_NUM, T); static uint32_t acceleration_time, deceleration_time;
#endif // !LIN_ADVANCE
static int32_t acceleration_time, deceleration_time;
static uint8_t step_loops, step_loops_nominal; static uint8_t step_loops, step_loops_nominal;
static hal_timer_t OCR1A_nominal; static uint32_t ticks_nominal;
#if DISABLED(BEZIER_JERK_CONTROL) #if DISABLED(BEZIER_JERK_CONTROL)
static hal_timer_t acc_step_rate; // needed for deceleration start point static uint32_t acc_step_rate; // needed for deceleration start point
#endif #endif
static volatile int32_t endstops_trigsteps[XYZ]; static volatile int32_t endstops_trigsteps[XYZ];
@ -167,88 +159,50 @@ class Stepper {
// //
Stepper() { }; Stepper() { };
//
// Initialize stepper hardware // Initialize stepper hardware
//
static void init(); static void init();
//
// Interrupt Service Routines // Interrupt Service Routines
//
static void isr(); // The ISR scheduler
static hal_timer_t isr_scheduler();
// The stepper pulse phase ISR
static void stepper_pulse_phase_isr();
// The stepper block processing phase ISR
static uint32_t stepper_block_phase_isr();
#if ENABLED(LIN_ADVANCE) #if ENABLED(LIN_ADVANCE)
static void advance_isr(); // The Linear advance stepper ISR
static void advance_isr_scheduler(); static uint32_t advance_isr();
#endif #endif
//
// Set the current position in steps
//
static void _set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e);
FORCE_INLINE static void _set_position(const AxisEnum a, const int32_t &v) { count_position[a] = v; }
FORCE_INLINE static void set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e) {
planner.synchronize();
CRITICAL_SECTION_START;
_set_position(a, b, c, e);
CRITICAL_SECTION_END;
}
static void set_position(const AxisEnum a, const int32_t &v) {
planner.synchronize();
CRITICAL_SECTION_START;
count_position[a] = v;
CRITICAL_SECTION_END;
}
FORCE_INLINE static void _set_e_position(const int32_t &e) { count_position[E_AXIS] = e; }
static void set_e_position(const int32_t &e) {
planner.synchronize();
CRITICAL_SECTION_START;
count_position[E_AXIS] = e;
CRITICAL_SECTION_END;
}
//
// Set direction bits for all steppers
//
static void set_directions();
//
// Get the position of a stepper, in steps // Get the position of a stepper, in steps
//
static int32_t position(const AxisEnum axis); static int32_t position(const AxisEnum axis);
//
// Report the positions of the steppers, in steps // Report the positions of the steppers, in steps
//
static void report_positions(); static void report_positions();
//
// The stepper subsystem goes to sleep when it runs out of things to execute. Call this // The stepper subsystem goes to sleep when it runs out of things to execute. Call this
// to notify the subsystem that it is time to go to work. // to notify the subsystem that it is time to go to work.
//
static void wake_up(); static void wake_up();
// // Quickly stop all steppers
// Wait for moves to finish and disable all steppers
//
static void finish_and_disable();
//
// Quickly stop all steppers and clear the blocks queue
//
static void quick_stop(); static void quick_stop();
//
// The direction of a single motor // The direction of a single motor
//
FORCE_INLINE static bool motor_direction(const AxisEnum axis) { return TEST(last_direction_bits, axis); } FORCE_INLINE static bool motor_direction(const AxisEnum axis) { return TEST(last_direction_bits, axis); }
// Kill current block
static void kill_current_block();
// Handle a triggered endstop
static void endstop_triggered(const AxisEnum axis);
// Triggered position of an axis in steps
static int32_t triggered_position(const AxisEnum axis);
#if HAS_DIGIPOTSS || HAS_MOTOR_CURRENT_PWM #if HAS_DIGIPOTSS || HAS_MOTOR_CURRENT_PWM
static void digitalPotWrite(const int16_t address, const int16_t value); static void digitalPotWrite(const int16_t address, const int16_t value);
static void digipot_current(const uint8_t driver, const int16_t current); static void digipot_current(const uint8_t driver, const int16_t current);
@ -280,34 +234,24 @@ class Stepper {
static void babystep(const AxisEnum axis, const bool direction); // perform a short step with a single stepper motor, outside of any convention static void babystep(const AxisEnum axis, const bool direction); // perform a short step with a single stepper motor, outside of any convention
#endif #endif
static inline void kill_current_block() {
step_events_completed = current_block->step_event_count;
}
//
// Handle a triggered endstop
//
static void endstop_triggered(const AxisEnum axis);
//
// Triggered position of an axis in mm (not core-savvy)
//
FORCE_INLINE static float triggered_position_mm(const AxisEnum axis) {
return endstops_trigsteps[axis] * planner.steps_to_mm[axis];
}
#if HAS_MOTOR_CURRENT_PWM #if HAS_MOTOR_CURRENT_PWM
static void refresh_motor_power(); static void refresh_motor_power();
#endif #endif
private: private:
FORCE_INLINE static hal_timer_t calc_timer_interval(hal_timer_t step_rate) { // Set the current position in steps
hal_timer_t timer; static void _set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e);
// Set direction bits for all steppers
static void set_directions();
FORCE_INLINE static uint32_t calc_timer_interval(uint32_t step_rate) {
uint32_t timer;
NOMORE(step_rate, MAX_STEP_FREQUENCY); NOMORE(step_rate, MAX_STEP_FREQUENCY);
// TODO: HAL: tidy this up, use condtionals_post.h // TODO: HAL: tidy this up, use Conditionals_post.h
#ifdef CPU_32_BIT #ifdef CPU_32_BIT
#if ENABLED(DISABLE_MULTI_STEPPING) #if ENABLED(DISABLE_MULTI_STEPPING)
step_loops = 1; step_loops = 1;
@ -348,16 +292,16 @@ class Stepper {
step_rate -= F_CPU / 500000; // Correct for minimal speed step_rate -= F_CPU / 500000; // Correct for minimal speed
if (step_rate >= (8 * 256)) { // higher step rate if (step_rate >= (8 * 256)) { // higher step rate
uint8_t tmp_step_rate = (step_rate & 0x00FF); uint8_t tmp_step_rate = (step_rate & 0x00FF);
uint16_t table_address = (uint16_t)&speed_lookuptable_fast[(uint8_t)(step_rate >> 8)][0]; uint16_t table_address = (uint16_t)&speed_lookuptable_fast[(uint8_t)(step_rate >> 8)][0],
uint16_t gain = (uint16_t)pgm_read_word_near(table_address + 2); gain = (uint16_t)pgm_read_word_near(table_address + 2);
timer = MultiU16X8toH16(tmp_step_rate, gain); timer = MultiU16X8toH16(tmp_step_rate, gain);
timer = (uint16_t)pgm_read_word_near(table_address) - timer; timer = (uint16_t)pgm_read_word_near(table_address) - timer;
} }
else { // lower step rates else { // lower step rates
uint16_t table_address = (uint16_t)&speed_lookuptable_slow[0][0]; uint16_t table_address = (uint16_t)&speed_lookuptable_slow[0][0];
table_address += ((step_rate) >> 1) & 0xFFFC; table_address += ((step_rate) >> 1) & 0xFFFC;
timer = (uint16_t)pgm_read_word_near(table_address); timer = (uint16_t)pgm_read_word_near(table_address)
timer -= (((uint16_t)pgm_read_word_near(table_address + 2) * (uint8_t)(step_rate & 0x0007)) >> 3); - (((uint16_t)pgm_read_word_near(table_address + 2) * (uint8_t)(step_rate & 0x0007)) >> 3);
} }
if (timer < 100) { // (20kHz - this should never happen) if (timer < 100) { // (20kHz - this should never happen)
timer = 100; timer = 100;

View file

@ -25,6 +25,7 @@
*/ */
#include "temperature.h" #include "temperature.h"
#include "endstops.h"
#include "../Marlin.h" #include "../Marlin.h"
#include "../lcd/ultralcd.h" #include "../lcd/ultralcd.h"
@ -1725,6 +1726,7 @@ void Temperature::set_current_temp_raw() {
* - Step the babysteps value for each axis towards 0 * - Step the babysteps value for each axis towards 0
* - For PINS_DEBUGGING, monitor and report endstop pins * - For PINS_DEBUGGING, monitor and report endstop pins
* - For ENDSTOP_INTERRUPTS_FEATURE check endstops if flagged * - For ENDSTOP_INTERRUPTS_FEATURE check endstops if flagged
* - Call planner.tick to count down its "ignore" time
*/ */
HAL_TEMP_TIMER_ISR { HAL_TEMP_TIMER_ISR {
HAL_timer_isr_prologue(TEMP_TIMER_NUM); HAL_timer_isr_prologue(TEMP_TIMER_NUM);
@ -2249,15 +2251,19 @@ void Temperature::isr() {
endstops.run_monitor(); // report changes in endstop status endstops.run_monitor(); // report changes in endstop status
#endif #endif
// Update endstops state, if enabled
#if ENABLED(ENDSTOP_INTERRUPTS_FEATURE) #if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
extern volatile uint8_t e_hit; extern volatile uint8_t e_hit;
if (e_hit && ENDSTOPS_ENABLED) { if (e_hit && ENDSTOPS_ENABLED) {
endstops.update(); // call endstop update routine endstops.update();
e_hit--; e_hit--;
} }
#else
if (ENDSTOPS_ENABLED) endstops.update();
#endif #endif
// Periodically call the planner timer
planner.tick();
} }
#if HAS_TEMP_SENSOR #if HAS_TEMP_SENSOR

View file

@ -29,7 +29,6 @@
#include "../Marlin.h" #include "../Marlin.h"
#include "../lcd/ultralcd.h" #include "../lcd/ultralcd.h"
#include "../module/planner.h" #include "../module/planner.h"
#include "../module/stepper.h"
#include "../module/printcounter.h" #include "../module/printcounter.h"
#include "../core/language.h" #include "../core/language.h"
#include "../gcode/queue.h" #include "../gcode/queue.h"
@ -983,7 +982,7 @@ void CardReader::printingHasFinished() {
#endif #endif
#if ENABLED(SD_FINISHED_STEPPERRELEASE) && defined(SD_FINISHED_RELEASECOMMAND) #if ENABLED(SD_FINISHED_STEPPERRELEASE) && defined(SD_FINISHED_RELEASECOMMAND)
stepper.cleaning_buffer_counter = 1; // The command will fire from the Stepper ISR planner.finish_and_disable();
#endif #endif
print_job_timer.stop(); print_job_timer.stop();
if (print_job_timer.duration() > 60) if (print_job_timer.duration() > 60)