Clean up stepper and babystep (#16857)
This commit is contained in:
Scott Lahteine
GitHub
parent
073e4443e8
commit
0b984519c3
@ -153,8 +153,8 @@ void stepperTask(void* parameter) {
|
||||
remaining--;
|
||||
}
|
||||
else {
|
||||
Stepper::stepper_pulse_phase_isr();
|
||||
remaining = Stepper::stepper_block_phase_isr();
|
||||
Stepper::pulse_phase_isr();
|
||||
remaining = Stepper::block_phase_isr();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -49,14 +49,6 @@ void Babystep::step_axis(const AxisEnum axis) {
|
||||
}
|
||||
}
|
||||
|
||||
void Babystep::task() {
|
||||
#if EITHER(BABYSTEP_XY, I2C_POSITION_ENCODERS)
|
||||
LOOP_XYZ(axis) step_axis((AxisEnum)axis);
|
||||
#else
|
||||
step_axis(Z_AXIS);
|
||||
#endif
|
||||
}
|
||||
|
||||
void Babystep::add_mm(const AxisEnum axis, const float &mm) {
|
||||
add_steps(axis, mm * planner.settings.axis_steps_per_mm[axis]);
|
||||
}
|
||||
|
||||
@ -55,7 +55,15 @@ public:
|
||||
|
||||
static void add_steps(const AxisEnum axis, const int16_t distance);
|
||||
static void add_mm(const AxisEnum axis, const float &mm);
|
||||
static void task();
|
||||
|
||||
//
|
||||
// Called by the Temperature ISR to
|
||||
// apply accumulated babysteps to the axes.
|
||||
//
|
||||
static inline void task() {
|
||||
LOOP_L_N(axis, BS_TODO_AXIS(Z_AXIS)) step_axis((AxisEnum)axis);
|
||||
}
|
||||
|
||||
private:
|
||||
static void step_axis(const AxisEnum axis);
|
||||
};
|
||||
|
||||
@ -709,6 +709,59 @@ void Planner::init() {
|
||||
|
||||
#define MINIMAL_STEP_RATE 120
|
||||
|
||||
/**
|
||||
* Get the current block for processing
|
||||
* and mark the block as busy.
|
||||
* Return nullptr if the buffer is empty
|
||||
* or if there is a first-block delay.
|
||||
*
|
||||
* WARNING: Called from Stepper ISR context!
|
||||
*/
|
||||
block_t* Planner::get_current_block() {
|
||||
// Get the number of moves in the planner queue so far
|
||||
const uint8_t nr_moves = movesplanned();
|
||||
|
||||
// If there are any moves queued ...
|
||||
if (nr_moves) {
|
||||
|
||||
// If there is still delay of delivery of blocks running, decrement it
|
||||
if (delay_before_delivering) {
|
||||
--delay_before_delivering;
|
||||
// If the number of movements queued is less than 3, and there is still time
|
||||
// to wait, do not deliver anything
|
||||
if (nr_moves < 3 && delay_before_delivering) return nullptr;
|
||||
delay_before_delivering = 0;
|
||||
}
|
||||
|
||||
// If we are here, there is no excuse to deliver the block
|
||||
block_t * const block = &block_buffer[block_buffer_tail];
|
||||
|
||||
// No trapezoid calculated? Don't execute yet.
|
||||
if (TEST(block->flag, BLOCK_BIT_RECALCULATE)) return nullptr;
|
||||
|
||||
#if HAS_SPI_LCD
|
||||
block_buffer_runtime_us -= block->segment_time_us; // We can't be sure how long an active block will take, so don't count it.
|
||||
#endif
|
||||
|
||||
// As this block is busy, advance the nonbusy block pointer
|
||||
block_buffer_nonbusy = next_block_index(block_buffer_tail);
|
||||
|
||||
// Push block_buffer_planned pointer, if encountered.
|
||||
if (block_buffer_tail == block_buffer_planned)
|
||||
block_buffer_planned = block_buffer_nonbusy;
|
||||
|
||||
// Return the block
|
||||
return block;
|
||||
}
|
||||
|
||||
// The queue became empty
|
||||
#if HAS_SPI_LCD
|
||||
clear_block_buffer_runtime(); // paranoia. Buffer is empty now - so reset accumulated time to zero.
|
||||
#endif
|
||||
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate trapezoid parameters, multiplying the entry- and exit-speeds
|
||||
* by the provided factors.
|
||||
@ -1498,8 +1551,7 @@ void Planner::quick_stop() {
|
||||
// must be handled: The tail could change between the read and the assignment
|
||||
// so this must be enclosed in a critical section
|
||||
|
||||
const bool was_enabled = STEPPER_ISR_ENABLED();
|
||||
if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
|
||||
const bool was_enabled = stepper.suspend();
|
||||
|
||||
// Drop all queue entries
|
||||
block_buffer_nonbusy = block_buffer_planned = block_buffer_head = block_buffer_tail;
|
||||
@ -1517,7 +1569,7 @@ void Planner::quick_stop() {
|
||||
cleaning_buffer_counter = 1000;
|
||||
|
||||
// Reenable Stepper ISR
|
||||
if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
|
||||
if (was_enabled) stepper.wake_up();
|
||||
|
||||
// And stop the stepper ISR
|
||||
stepper.quick_stop();
|
||||
@ -1548,13 +1600,12 @@ float Planner::get_axis_position_mm(const AxisEnum axis) {
|
||||
if (axis == CORE_AXIS_1 || axis == CORE_AXIS_2) {
|
||||
|
||||
// Protect the access to the position.
|
||||
const bool was_enabled = STEPPER_ISR_ENABLED();
|
||||
if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
|
||||
const bool was_enabled = stepper.suspend();
|
||||
|
||||
const int32_t p1 = stepper.position(CORE_AXIS_1),
|
||||
p2 = stepper.position(CORE_AXIS_2);
|
||||
|
||||
if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
|
||||
if (was_enabled) stepper.wake_up();
|
||||
|
||||
// ((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
|
||||
@ -2004,13 +2055,12 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
|
||||
|
||||
#if HAS_SPI_LCD
|
||||
// Protect the access to the position.
|
||||
const bool was_enabled = STEPPER_ISR_ENABLED();
|
||||
if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
|
||||
const bool was_enabled = stepper.suspend();
|
||||
|
||||
block_buffer_runtime_us += segment_time_us;
|
||||
block->segment_time_us = segment_time_us;
|
||||
|
||||
if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
|
||||
if (was_enabled) stepper.wake_up();
|
||||
#endif
|
||||
|
||||
block->nominal_speed_sqr = sq(block->millimeters * inverse_secs); // (mm/sec)^2 Always > 0
|
||||
@ -2822,6 +2872,48 @@ void Planner::set_max_jerk(const AxisEnum axis, float targetValue) {
|
||||
#endif
|
||||
}
|
||||
|
||||
#if HAS_SPI_LCD
|
||||
|
||||
uint16_t Planner::block_buffer_runtime() {
|
||||
#ifdef __AVR__
|
||||
// Protect the access to the variable. Only required for AVR, as
|
||||
// any 32bit CPU offers atomic access to 32bit variables
|
||||
const bool was_enabled = stepper.suspend();
|
||||
#endif
|
||||
|
||||
millis_t bbru = block_buffer_runtime_us;
|
||||
|
||||
#ifdef __AVR__
|
||||
// Reenable Stepper ISR
|
||||
if (was_enabled) stepper.wake_up();
|
||||
#endif
|
||||
|
||||
// To translate µs to ms a division by 1000 would be required.
|
||||
// We introduce 2.4% error here by dividing by 1024.
|
||||
// Doesn't matter because block_buffer_runtime_us is already too small an estimation.
|
||||
bbru >>= 10;
|
||||
// limit to about a minute.
|
||||
NOMORE(bbru, 0xFFFFul);
|
||||
return bbru;
|
||||
}
|
||||
|
||||
void Planner::clear_block_buffer_runtime() {
|
||||
#ifdef __AVR__
|
||||
// Protect the access to the variable. Only required for AVR, as
|
||||
// any 32bit CPU offers atomic access to 32bit variables
|
||||
const bool was_enabled = stepper.suspend();
|
||||
#endif
|
||||
|
||||
block_buffer_runtime_us = 0;
|
||||
|
||||
#ifdef __AVR__
|
||||
// Reenable Stepper ISR
|
||||
if (was_enabled) stepper.wake_up();
|
||||
#endif
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
#if ENABLED(AUTOTEMP)
|
||||
|
||||
void Planner::autotemp_M104_M109() {
|
||||
|
||||
@ -763,60 +763,18 @@ class Planner {
|
||||
FORCE_INLINE static bool has_blocks_queued() { return (block_buffer_head != block_buffer_tail); }
|
||||
|
||||
/**
|
||||
* The current block. nullptr if the buffer is empty.
|
||||
* This also marks the block as busy.
|
||||
* Get the current block for processing
|
||||
* and mark the block as busy.
|
||||
* Return nullptr if the buffer is empty
|
||||
* or if there is a first-block delay.
|
||||
*
|
||||
* WARNING: Called from Stepper ISR context!
|
||||
*/
|
||||
static block_t* get_current_block() {
|
||||
|
||||
// Get the number of moves in the planner queue so far
|
||||
const uint8_t nr_moves = movesplanned();
|
||||
|
||||
// If there are any moves queued ...
|
||||
if (nr_moves) {
|
||||
|
||||
// If there is still delay of delivery of blocks running, decrement it
|
||||
if (delay_before_delivering) {
|
||||
--delay_before_delivering;
|
||||
// If the number of movements queued is less than 3, and there is still time
|
||||
// to wait, do not deliver anything
|
||||
if (nr_moves < 3 && delay_before_delivering) return nullptr;
|
||||
delay_before_delivering = 0;
|
||||
}
|
||||
|
||||
// If we are here, there is no excuse to deliver the block
|
||||
block_t * const block = &block_buffer[block_buffer_tail];
|
||||
|
||||
// No trapezoid calculated? Don't execute yet.
|
||||
if (TEST(block->flag, BLOCK_BIT_RECALCULATE)) return nullptr;
|
||||
|
||||
#if HAS_SPI_LCD
|
||||
block_buffer_runtime_us -= block->segment_time_us; // We can't be sure how long an active block will take, so don't count it.
|
||||
#endif
|
||||
|
||||
// As this block is busy, advance the nonbusy block pointer
|
||||
block_buffer_nonbusy = next_block_index(block_buffer_tail);
|
||||
|
||||
// Push block_buffer_planned pointer, if encountered.
|
||||
if (block_buffer_tail == block_buffer_planned)
|
||||
block_buffer_planned = block_buffer_nonbusy;
|
||||
|
||||
// Return the block
|
||||
return block;
|
||||
}
|
||||
|
||||
// The queue became empty
|
||||
#if HAS_SPI_LCD
|
||||
clear_block_buffer_runtime(); // paranoia. Buffer is empty now - so reset accumulated time to zero.
|
||||
#endif
|
||||
|
||||
return nullptr;
|
||||
}
|
||||
static block_t* get_current_block();
|
||||
|
||||
/**
|
||||
* "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() {
|
||||
if (has_blocks_queued())
|
||||
@ -824,47 +782,8 @@ class Planner {
|
||||
}
|
||||
|
||||
#if HAS_SPI_LCD
|
||||
|
||||
static uint16_t block_buffer_runtime() {
|
||||
#ifdef __AVR__
|
||||
// Protect the access to the variable. Only required for AVR, as
|
||||
// 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.
|
||||
// We introduce 2.4% error here by dividing by 1024.
|
||||
// Doesn't matter because block_buffer_runtime_us is already too small an estimation.
|
||||
bbru >>= 10;
|
||||
// limit to about a minute.
|
||||
NOMORE(bbru, 0xFFFFul);
|
||||
return bbru;
|
||||
}
|
||||
|
||||
static void clear_block_buffer_runtime() {
|
||||
#ifdef __AVR__
|
||||
// Protect the access to the variable. Only required for AVR, as
|
||||
// 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
|
||||
}
|
||||
|
||||
static uint16_t block_buffer_runtime();
|
||||
static void clear_block_buffer_runtime();
|
||||
#endif
|
||||
|
||||
#if ENABLED(AUTOTEMP)
|
||||
|
||||
@ -203,11 +203,8 @@ uint32_t Stepper::advance_divisor = 0,
|
||||
bool Stepper::bezier_2nd_half; // =false If Bézier curve has been initialized or not
|
||||
#endif
|
||||
|
||||
uint32_t Stepper::nextMainISR = 0;
|
||||
|
||||
#if ENABLED(LIN_ADVANCE)
|
||||
|
||||
constexpr uint32_t LA_ADV_NEVER = 0xFFFFFFFF;
|
||||
uint32_t Stepper::nextAdvanceISR = LA_ADV_NEVER,
|
||||
Stepper::LA_isr_rate = LA_ADV_NEVER;
|
||||
uint16_t Stepper::LA_current_adv_steps = 0,
|
||||
@ -402,13 +399,13 @@ constexpr uint32_t NS_TO_PULSE_TIMER_TICKS(uint32_t NS) { return (NS + (NS_PER_P
|
||||
#define PULSE_HIGH_TICK_COUNT hal_timer_t(NS_TO_PULSE_TIMER_TICKS(_MIN_PULSE_HIGH_NS - _MIN(_MIN_PULSE_HIGH_NS, TIMER_SETUP_NS)))
|
||||
#define PULSE_LOW_TICK_COUNT hal_timer_t(NS_TO_PULSE_TIMER_TICKS(_MIN_PULSE_LOW_NS - _MIN(_MIN_PULSE_LOW_NS, TIMER_SETUP_NS)))
|
||||
|
||||
#define USING_TIMED_PULSE() hal_timer_t end_tick_count = 0
|
||||
#define START_TIMED_PULSE(DIR) (end_tick_count = HAL_timer_get_count(PULSE_TIMER_NUM) + PULSE_##DIR##_TICK_COUNT)
|
||||
#define AWAIT_TIMED_PULSE() while (HAL_timer_get_count(PULSE_TIMER_NUM) < end_tick_count) { }
|
||||
#define USING_TIMED_PULSE() hal_timer_t start_pulse_count = 0
|
||||
#define START_TIMED_PULSE(DIR) (start_pulse_count = HAL_timer_get_count(PULSE_TIMER_NUM))
|
||||
#define AWAIT_TIMED_PULSE(DIR) while (PULSE_##DIR##_TICK_COUNT > HAL_timer_get_count(PULSE_TIMER_NUM) - start_pulse_count) { }
|
||||
#define START_HIGH_PULSE() START_TIMED_PULSE(HIGH)
|
||||
#define AWAIT_HIGH_PULSE() AWAIT_TIMED_PULSE(HIGH)
|
||||
#define START_LOW_PULSE() START_TIMED_PULSE(LOW)
|
||||
#define AWAIT_HIGH_PULSE() AWAIT_TIMED_PULSE()
|
||||
#define AWAIT_LOW_PULSE() AWAIT_TIMED_PULSE()
|
||||
#define AWAIT_LOW_PULSE() AWAIT_TIMED_PULSE(LOW)
|
||||
|
||||
#if MINIMUM_STEPPER_PRE_DIR_DELAY > 0
|
||||
#define DIR_WAIT_BEFORE() DELAY_NS(MINIMUM_STEPPER_PRE_DIR_DELAY)
|
||||
@ -422,11 +419,6 @@ constexpr uint32_t NS_TO_PULSE_TIMER_TICKS(uint32_t NS) { return (NS + (NS_PER_P
|
||||
#define DIR_WAIT_AFTER()
|
||||
#endif
|
||||
|
||||
void Stepper::wake_up() {
|
||||
// TCNT1 = 0;
|
||||
ENABLE_STEPPER_DRIVER_INTERRUPT();
|
||||
}
|
||||
|
||||
/**
|
||||
* Set the stepper direction of each axis
|
||||
*
|
||||
@ -1334,6 +1326,9 @@ HAL_STEP_TIMER_ISR() {
|
||||
#endif
|
||||
|
||||
void Stepper::isr() {
|
||||
|
||||
static uint32_t nextMainISR = 0; // Interval until the next main Stepper Pulse phase (0 = Now)
|
||||
|
||||
#ifndef __AVR__
|
||||
// Disable interrupts, to avoid ISR preemption while we reprogram the period
|
||||
// (AVR enters the ISR with global interrupts disabled, so no need to do it here)
|
||||
@ -1357,35 +1352,35 @@ void Stepper::isr() {
|
||||
// Enable ISRs to reduce USART processing latency
|
||||
ENABLE_ISRS();
|
||||
|
||||
// Run main stepping pulse phase ISR if we have to
|
||||
if (!nextMainISR) Stepper::stepper_pulse_phase_isr();
|
||||
if (!nextMainISR) pulse_phase_isr(); // 0 = Do coordinated axes Stepper pulses
|
||||
|
||||
#if ENABLED(LIN_ADVANCE)
|
||||
// Run linear advance stepper ISR if we have to
|
||||
if (!nextAdvanceISR) nextAdvanceISR = Stepper::advance_isr();
|
||||
if (!nextAdvanceISR) nextAdvanceISR = advance_isr(); // 0 = Do Linear Advance E Stepper pulses
|
||||
#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 (!nextMainISR) nextMainISR = block_phase_isr(); // Manage acc/deceleration, get next block
|
||||
|
||||
uint32_t interval =
|
||||
// Get the interval to the next ISR call
|
||||
const uint32_t interval = _MIN(
|
||||
nextMainISR // Time until the next Stepper ISR
|
||||
#if ENABLED(LIN_ADVANCE)
|
||||
_MIN(nextAdvanceISR, nextMainISR) // Nearest time interval
|
||||
#else
|
||||
nextMainISR // Remaining stepper ISR time
|
||||
, nextAdvanceISR // Come back early for Linear Advance?
|
||||
#endif
|
||||
;
|
||||
, uint32_t(HAL_TIMER_TYPE_MAX) // Come back in a very long time
|
||||
);
|
||||
|
||||
// Limit the value to the maximum possible value of the timer
|
||||
NOMORE(interval, uint32_t(HAL_TIMER_TYPE_MAX));
|
||||
//
|
||||
// Compute remaining time for each ISR phase
|
||||
// NEVER : The phase is idle
|
||||
// Zero : The phase will occur on the next ISR call
|
||||
// Non-zero : The phase will occur on a future ISR call
|
||||
//
|
||||
|
||||
// Compute the time remaining for the main isr
|
||||
nextMainISR -= interval;
|
||||
|
||||
#if ENABLED(LIN_ADVANCE)
|
||||
// Compute the time remaining for the advance isr
|
||||
if (nextAdvanceISR != LA_ADV_NEVER) nextAdvanceISR -= interval;
|
||||
#endif
|
||||
|
||||
@ -1471,7 +1466,7 @@ void Stepper::isr() {
|
||||
* call to this method that might cause variation in the timing. The aim
|
||||
* is to keep pulse timing as regular as possible.
|
||||
*/
|
||||
void Stepper::stepper_pulse_phase_isr() {
|
||||
void Stepper::pulse_phase_isr() {
|
||||
|
||||
// If we must abort the current block, do so!
|
||||
if (abort_current_block) {
|
||||
@ -1548,7 +1543,7 @@ void Stepper::stepper_pulse_phase_isr() {
|
||||
// Don't step E here - But remember the number of steps to perform
|
||||
motor_direction(E_AXIS) ? --LA_steps : ++LA_steps;
|
||||
#else
|
||||
step_needed.e = delta_error.e >= 0;
|
||||
step_needed.e = true;
|
||||
#endif
|
||||
}
|
||||
#elif HAS_E0_STEP
|
||||
@ -1604,20 +1599,14 @@ void Stepper::stepper_pulse_phase_isr() {
|
||||
|
||||
#if DISABLED(LIN_ADVANCE)
|
||||
#if ENABLED(MIXING_EXTRUDER)
|
||||
|
||||
if (delta_error.e >= 0) {
|
||||
delta_error.e -= advance_divisor;
|
||||
E_STEP_WRITE(mixer.get_stepper(), INVERT_E_STEP_PIN);
|
||||
}
|
||||
|
||||
#else // !MIXING_EXTRUDER
|
||||
|
||||
#if HAS_E0_STEP
|
||||
PULSE_STOP(E);
|
||||
#endif
|
||||
|
||||
#endif // !MIXING_EXTRUDER
|
||||
#endif // !LIN_ADVANCE
|
||||
#elif HAS_E0_STEP
|
||||
PULSE_STOP(E);
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#if ISR_MULTI_STEPS
|
||||
if (events_to_do) START_LOW_PULSE();
|
||||
@ -1630,10 +1619,10 @@ void Stepper::stepper_pulse_phase_isr() {
|
||||
// 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.
|
||||
|
||||
uint32_t Stepper::stepper_block_phase_isr() {
|
||||
uint32_t Stepper::block_phase_isr() {
|
||||
|
||||
// If no queued movements, just wait 1ms for the next move
|
||||
uint32_t interval = (STEPPER_TIMER_RATE) / 1000;
|
||||
// If no queued movements, just wait 1ms for the next block
|
||||
uint32_t interval = (STEPPER_TIMER_RATE) / 1000UL;
|
||||
|
||||
// If there is a current block
|
||||
if (current_block) {
|
||||
@ -1667,16 +1656,14 @@ uint32_t Stepper::stepper_block_phase_isr() {
|
||||
// acc_step_rate is in steps/second
|
||||
|
||||
// step_rate to timer interval and steps per stepper isr
|
||||
interval = calc_timer_interval(acc_step_rate, oversampling_factor, &steps_per_isr);
|
||||
interval = calc_timer_interval(acc_step_rate, &steps_per_isr);
|
||||
acceleration_time += interval;
|
||||
|
||||
#if ENABLED(LIN_ADVANCE)
|
||||
if (LA_use_advance_lead) {
|
||||
// Fire ISR if final adv_rate is reached
|
||||
if (LA_steps && LA_isr_rate != current_block->advance_speed) nextAdvanceISR = 0;
|
||||
}
|
||||
else if (LA_steps) nextAdvanceISR = 0;
|
||||
#endif // LIN_ADVANCE
|
||||
// Fire ISR if final adv_rate is reached
|
||||
if (LA_steps && (!LA_use_advance_lead || LA_isr_rate != current_block->advance_speed))
|
||||
initiateLA();
|
||||
#endif
|
||||
}
|
||||
// Are we in Deceleration phase ?
|
||||
else if (step_events_completed > decelerate_after) {
|
||||
@ -1712,32 +1699,32 @@ uint32_t Stepper::stepper_block_phase_isr() {
|
||||
// step_rate is in steps/second
|
||||
|
||||
// step_rate to timer interval and steps per stepper isr
|
||||
interval = calc_timer_interval(step_rate, oversampling_factor, &steps_per_isr);
|
||||
interval = calc_timer_interval(step_rate, &steps_per_isr);
|
||||
deceleration_time += interval;
|
||||
|
||||
#if ENABLED(LIN_ADVANCE)
|
||||
if (LA_use_advance_lead) {
|
||||
// Wake up eISR on first deceleration loop and fire ISR if final adv_rate is reached
|
||||
if (step_events_completed <= decelerate_after + steps_per_isr || (LA_steps && LA_isr_rate != current_block->advance_speed)) {
|
||||
nextAdvanceISR = 0;
|
||||
initiateLA();
|
||||
LA_isr_rate = current_block->advance_speed;
|
||||
}
|
||||
}
|
||||
else if (LA_steps) nextAdvanceISR = 0;
|
||||
#endif // LIN_ADVANCE
|
||||
else if (LA_steps) initiateLA();
|
||||
#endif
|
||||
}
|
||||
// We must be in cruise phase otherwise
|
||||
else {
|
||||
|
||||
#if ENABLED(LIN_ADVANCE)
|
||||
// If there are any esteps, fire the next advance_isr "now"
|
||||
if (LA_steps && LA_isr_rate != current_block->advance_speed) nextAdvanceISR = 0;
|
||||
if (LA_steps && LA_isr_rate != current_block->advance_speed) initiateLA();
|
||||
#endif
|
||||
|
||||
// Calculate the ticks_nominal for this nominal speed, if not done yet
|
||||
if (ticks_nominal < 0) {
|
||||
// step_rate to timer interval and loops for the nominal speed
|
||||
ticks_nominal = calc_timer_interval(current_block->nominal_rate, oversampling_factor, &steps_per_isr);
|
||||
ticks_nominal = calc_timer_interval(current_block->nominal_rate, &steps_per_isr);
|
||||
}
|
||||
|
||||
// The timer interval is just the nominal value for the nominal speed
|
||||
@ -1846,17 +1833,17 @@ uint32_t Stepper::stepper_block_phase_isr() {
|
||||
// No acceleration / deceleration time elapsed so far
|
||||
acceleration_time = deceleration_time = 0;
|
||||
|
||||
uint8_t oversampling = 0; // Assume we won't use it
|
||||
uint8_t oversampling = 0; // Assume no axis smoothing (via oversampling)
|
||||
|
||||
#if ENABLED(ADAPTIVE_STEP_SMOOTHING)
|
||||
// At this point, we must decide if we can use Stepper movement axis smoothing.
|
||||
// Decide if axis smoothing is possible
|
||||
uint32_t max_rate = current_block->nominal_rate; // Get the maximum rate (maximum event speed)
|
||||
while (max_rate < MIN_STEP_ISR_FREQUENCY) {
|
||||
max_rate <<= 1;
|
||||
if (max_rate >= MAX_STEP_ISR_FREQUENCY_1X) break;
|
||||
++oversampling;
|
||||
while (max_rate < MIN_STEP_ISR_FREQUENCY) { // As long as more ISRs are possible...
|
||||
max_rate <<= 1; // Try to double the rate
|
||||
if (max_rate >= MAX_STEP_ISR_FREQUENCY_1X) break; // Don't exceed the estimated ISR limit
|
||||
++oversampling; // Increase the oversampling (used for left-shift)
|
||||
}
|
||||
oversampling_factor = oversampling;
|
||||
oversampling_factor = oversampling; // For all timer interval calculations
|
||||
#endif
|
||||
|
||||
// Based on the oversampling factor, do the calculations
|
||||
@ -1894,8 +1881,7 @@ uint32_t Stepper::stepper_block_phase_isr() {
|
||||
if ((LA_use_advance_lead = current_block->use_advance_lead)) {
|
||||
LA_final_adv_steps = current_block->final_adv_steps;
|
||||
LA_max_adv_steps = current_block->max_adv_steps;
|
||||
//Start the ISR
|
||||
nextAdvanceISR = 0;
|
||||
initiateLA(); // Start the ISR
|
||||
LA_isr_rate = current_block->advance_speed;
|
||||
}
|
||||
else LA_isr_rate = LA_ADV_NEVER;
|
||||
@ -1954,7 +1940,7 @@ uint32_t Stepper::stepper_block_phase_isr() {
|
||||
#endif
|
||||
|
||||
// Calculate the initial timer interval
|
||||
interval = calc_timer_interval(current_block->initial_rate, oversampling_factor, &steps_per_isr);
|
||||
interval = calc_timer_interval(current_block->initial_rate, &steps_per_isr);
|
||||
}
|
||||
}
|
||||
|
||||
@ -2054,6 +2040,7 @@ uint32_t Stepper::stepper_block_phase_isr() {
|
||||
|
||||
return interval;
|
||||
}
|
||||
|
||||
#endif // LIN_ADVANCE
|
||||
|
||||
// Check if the given block is busy or not - Must not be called from ISR contexts
|
||||
@ -2093,7 +2080,7 @@ void Stepper::init() {
|
||||
digipot_motor = 255 * (motor_current[i] / 2.5);
|
||||
dac084s085::setValue(i, digipot_motor);
|
||||
}
|
||||
#endif//MB(ALLIGATOR)
|
||||
#endif
|
||||
|
||||
// Init Microstepping Pins
|
||||
#if HAS_MICROSTEPS
|
||||
@ -2287,7 +2274,7 @@ void Stepper::init() {
|
||||
|
||||
#if DISABLED(I2S_STEPPER_STREAM)
|
||||
HAL_timer_start(STEP_TIMER_NUM, 122); // Init Stepper ISR to 122 Hz for quick starting
|
||||
ENABLE_STEPPER_DRIVER_INTERRUPT();
|
||||
wake_up();
|
||||
sei();
|
||||
#endif
|
||||
|
||||
@ -2341,19 +2328,43 @@ int32_t Stepper::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();
|
||||
const bool was_enabled = suspend();
|
||||
#endif
|
||||
|
||||
const int32_t v = count_position[axis];
|
||||
|
||||
#ifdef __AVR__
|
||||
// Reenable Stepper ISR
|
||||
if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
|
||||
if (was_enabled) wake_up();
|
||||
#endif
|
||||
return v;
|
||||
}
|
||||
|
||||
// Set the current position in steps
|
||||
void Stepper::set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e) {
|
||||
planner.synchronize();
|
||||
const bool was_enabled = suspend();
|
||||
_set_position(a, b, c, e);
|
||||
if (was_enabled) wake_up();
|
||||
}
|
||||
|
||||
void Stepper::set_axis_position(const AxisEnum a, const int32_t &v) {
|
||||
planner.synchronize();
|
||||
|
||||
#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 = suspend();
|
||||
#endif
|
||||
|
||||
count_position[a] = v;
|
||||
|
||||
#ifdef __AVR__
|
||||
// Reenable Stepper ISR
|
||||
if (was_enabled) wake_up();
|
||||
#endif
|
||||
}
|
||||
|
||||
// Signal endstops were triggered - This function can be called from
|
||||
// an ISR context (Temperature, Stepper or limits ISR), so we must
|
||||
// be very careful here. If the interrupt being preempted was the
|
||||
@ -2362,8 +2373,7 @@ int32_t Stepper::position(const AxisEnum axis) {
|
||||
// is properly canceled
|
||||
void Stepper::endstop_triggered(const AxisEnum axis) {
|
||||
|
||||
const bool was_enabled = STEPPER_ISR_ENABLED();
|
||||
if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
|
||||
const bool was_enabled = suspend();
|
||||
endstops_trigsteps[axis] = (
|
||||
#if IS_CORE
|
||||
(axis == CORE_AXIS_2
|
||||
@ -2378,22 +2388,21 @@ void Stepper::endstop_triggered(const AxisEnum axis) {
|
||||
// Discard the rest of the move if there is a current block
|
||||
quick_stop();
|
||||
|
||||
if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
|
||||
if (was_enabled) wake_up();
|
||||
}
|
||||
|
||||
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();
|
||||
const bool was_enabled = suspend();
|
||||
#endif
|
||||
|
||||
const int32_t v = endstops_trigsteps[axis];
|
||||
|
||||
#ifdef __AVR__
|
||||
// Reenable Stepper ISR
|
||||
if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
|
||||
if (was_enabled) wake_up();
|
||||
#endif
|
||||
|
||||
return v;
|
||||
@ -2403,14 +2412,13 @@ void Stepper::report_positions() {
|
||||
|
||||
#ifdef __AVR__
|
||||
// Protect the access to the position.
|
||||
const bool was_enabled = STEPPER_ISR_ENABLED();
|
||||
if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
|
||||
const bool was_enabled = suspend();
|
||||
#endif
|
||||
|
||||
const xyz_long_t pos = count_position;
|
||||
|
||||
#ifdef __AVR__
|
||||
if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
|
||||
if (was_enabled) wake_up();
|
||||
#endif
|
||||
|
||||
#if CORE_IS_XY || CORE_IS_XZ || ENABLED(DELTA) || IS_SCARA
|
||||
@ -2571,16 +2579,21 @@ void Stepper::report_positions() {
|
||||
Z_STEP_WRITE(INVERT_Z_STEP_PIN);
|
||||
|
||||
// Restore direction bits
|
||||
DIR_WAIT_BEFORE();
|
||||
|
||||
X_DIR_WRITE(old_dir.x);
|
||||
Y_DIR_WRITE(old_dir.y);
|
||||
Z_DIR_WRITE(old_dir.z);
|
||||
|
||||
DIR_WAIT_AFTER();
|
||||
|
||||
#endif
|
||||
|
||||
} break;
|
||||
|
||||
default: break;
|
||||
}
|
||||
|
||||
sei();
|
||||
}
|
||||
|
||||
|
||||
@ -321,13 +321,13 @@ class Stepper {
|
||||
static bool bezier_2nd_half; // If Bézier curve has been initialized or not
|
||||
#endif
|
||||
|
||||
static uint32_t nextMainISR; // time remaining for the next Step ISR
|
||||
#if ENABLED(LIN_ADVANCE)
|
||||
static constexpr uint32_t LA_ADV_NEVER = 0xFFFFFFFF;
|
||||
static uint32_t nextAdvanceISR, LA_isr_rate;
|
||||
static uint16_t LA_current_adv_steps, LA_final_adv_steps, LA_max_adv_steps; // Copy from current executed block. Needed because current_block is set to NULL "too early".
|
||||
static int8_t LA_steps;
|
||||
static bool LA_use_advance_lead;
|
||||
#endif // LIN_ADVANCE
|
||||
#endif
|
||||
|
||||
static int32_t ticks_nominal;
|
||||
#if DISABLED(S_CURVE_ACCELERATION)
|
||||
@ -351,28 +351,36 @@ class Stepper {
|
||||
|
||||
public:
|
||||
|
||||
//
|
||||
// Constructor / initializer
|
||||
//
|
||||
Stepper() {};
|
||||
|
||||
// Initialize stepper hardware
|
||||
static void init();
|
||||
|
||||
// Interrupt Service Routines
|
||||
// Interrupt Service Routine and phases
|
||||
|
||||
// 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.
|
||||
static inline void wake_up() { ENABLE_STEPPER_DRIVER_INTERRUPT(); }
|
||||
|
||||
static inline bool is_awake() { return STEPPER_ISR_ENABLED(); }
|
||||
|
||||
static inline bool suspend() {
|
||||
const bool awake = is_awake();
|
||||
if (awake) DISABLE_STEPPER_DRIVER_INTERRUPT();
|
||||
return awake;
|
||||
}
|
||||
|
||||
// The ISR scheduler
|
||||
static void isr();
|
||||
|
||||
// The stepper pulse phase ISR
|
||||
static void stepper_pulse_phase_isr();
|
||||
// The stepper pulse ISR phase
|
||||
static void pulse_phase_isr();
|
||||
|
||||
// The stepper block processing phase ISR
|
||||
static uint32_t stepper_block_phase_isr();
|
||||
// The stepper block processing ISR phase
|
||||
static uint32_t block_phase_isr();
|
||||
|
||||
#if ENABLED(LIN_ADVANCE)
|
||||
// The Linear advance stepper ISR
|
||||
// The Linear advance ISR phase
|
||||
static uint32_t advance_isr();
|
||||
FORCE_INLINE static void initiateLA() { nextAdvanceISR = 0; }
|
||||
#endif
|
||||
|
||||
// Check if the given block is busy or not - Must not be called from ISR contexts
|
||||
@ -381,13 +389,14 @@ class Stepper {
|
||||
// Get the position of a stepper, in steps
|
||||
static int32_t position(const AxisEnum axis);
|
||||
|
||||
// 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);
|
||||
static inline void set_position(const xyze_long_t &abce) { set_position(abce.a, abce.b, abce.c, abce.e); }
|
||||
static void set_axis_position(const AxisEnum a, const int32_t &v);
|
||||
|
||||
// Report the positions of the steppers, in steps
|
||||
static void report_positions();
|
||||
|
||||
// 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.
|
||||
static void wake_up();
|
||||
|
||||
// Quickly stop all steppers
|
||||
FORCE_INLINE static void quick_stop() { abort_current_block = true; }
|
||||
|
||||
@ -453,34 +462,6 @@ class Stepper {
|
||||
static void refresh_motor_power();
|
||||
#endif
|
||||
|
||||
// Set the current position in steps
|
||||
static inline void set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e) {
|
||||
planner.synchronize();
|
||||
const bool was_enabled = STEPPER_ISR_ENABLED();
|
||||
if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
|
||||
_set_position(a, b, c, e);
|
||||
if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
|
||||
}
|
||||
static inline void set_position(const xyze_long_t &abce) { set_position(abce.a, abce.b, abce.c, abce.e); }
|
||||
|
||||
static inline void set_axis_position(const AxisEnum a, const int32_t &v) {
|
||||
planner.synchronize();
|
||||
|
||||
#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
|
||||
|
||||
count_position[a] = v;
|
||||
|
||||
#ifdef __AVR__
|
||||
// Reenable Stepper ISR
|
||||
if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
|
||||
#endif
|
||||
}
|
||||
|
||||
// Set direction bits for all steppers
|
||||
static void set_directions();
|
||||
|
||||
@ -490,11 +471,11 @@ class Stepper {
|
||||
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 abce_long_t &spos) { _set_position(spos.a, spos.b, spos.c, spos.e); }
|
||||
|
||||
FORCE_INLINE static uint32_t calc_timer_interval(uint32_t step_rate, uint8_t scale, uint8_t* loops) {
|
||||
FORCE_INLINE static uint32_t calc_timer_interval(uint32_t step_rate, uint8_t* loops) {
|
||||
uint32_t timer;
|
||||
|
||||
// Scale the frequency, as requested by the caller
|
||||
step_rate <<= scale;
|
||||
step_rate <<= oversampling_factor;
|
||||
|
||||
uint8_t multistep = 1;
|
||||
#if DISABLED(DISABLE_MULTI_STEPPING)
|
||||
|
||||
@ -65,15 +65,12 @@
|
||||
#include "../libs/private_spi.h"
|
||||
#endif
|
||||
|
||||
#if EITHER(BABYSTEPPING, PID_EXTRUSION_SCALING)
|
||||
#if ENABLED(PID_EXTRUSION_SCALING)
|
||||
#include "stepper.h"
|
||||
#endif
|
||||
|
||||
#if ENABLED(BABYSTEPPING)
|
||||
#include "../feature/babystep.h"
|
||||
#if ENABLED(BABYSTEP_ALWAYS_AVAILABLE)
|
||||
#include "../gcode/gcode.h"
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#include "printcounter.h"
|
||||
|
||||
Loading…
Reference in New Issue
Block a user