|
|
@ -187,7 +187,7 @@ class Planner {
|
|
|
|
/**
|
|
|
|
/**
|
|
|
|
* Number of moves currently in the planner
|
|
|
|
* Number of moves currently in the planner
|
|
|
|
*/
|
|
|
|
*/
|
|
|
|
static FORCE_INLINE uint8_t movesplanned() { return BLOCK_MOD(block_buffer_head - block_buffer_tail + BLOCK_BUFFER_SIZE); }
|
|
|
|
static uint8_t movesplanned() { return BLOCK_MOD(block_buffer_head - block_buffer_tail + BLOCK_BUFFER_SIZE); }
|
|
|
|
|
|
|
|
|
|
|
|
#if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING)
|
|
|
|
#if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING)
|
|
|
|
|
|
|
|
|
|
|
@ -233,13 +233,13 @@ class Planner {
|
|
|
|
/**
|
|
|
|
/**
|
|
|
|
* Does the buffer have any blocks queued?
|
|
|
|
* Does the buffer have any blocks queued?
|
|
|
|
*/
|
|
|
|
*/
|
|
|
|
static FORCE_INLINE bool blocks_queued() { return (block_buffer_head != block_buffer_tail); }
|
|
|
|
static bool blocks_queued() { return (block_buffer_head != block_buffer_tail); }
|
|
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
/**
|
|
|
|
* "Discards" the block and "releases" the memory.
|
|
|
|
* "Discards" the block and "releases" the memory.
|
|
|
|
* Called when the current block is no longer needed.
|
|
|
|
* Called when the current block is no longer needed.
|
|
|
|
*/
|
|
|
|
*/
|
|
|
|
static FORCE_INLINE void discard_current_block() {
|
|
|
|
static void discard_current_block() {
|
|
|
|
if (blocks_queued())
|
|
|
|
if (blocks_queued())
|
|
|
|
block_buffer_tail = BLOCK_MOD(block_buffer_tail + 1);
|
|
|
|
block_buffer_tail = BLOCK_MOD(block_buffer_tail + 1);
|
|
|
|
}
|
|
|
|
}
|
|
|
@ -248,7 +248,7 @@ class Planner {
|
|
|
|
* 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.
|
|
|
|
*/
|
|
|
|
*/
|
|
|
|
static FORCE_INLINE block_t* get_current_block() {
|
|
|
|
static block_t* get_current_block() {
|
|
|
|
if (blocks_queued()) {
|
|
|
|
if (blocks_queued()) {
|
|
|
|
block_t* block = &block_buffer[block_buffer_tail];
|
|
|
|
block_t* block = &block_buffer[block_buffer_tail];
|
|
|
|
block->busy = true;
|
|
|
|
block->busy = true;
|
|
|
@ -272,14 +272,14 @@ class Planner {
|
|
|
|
/**
|
|
|
|
/**
|
|
|
|
* Get the index of the next / previous block in the ring buffer
|
|
|
|
* Get the index of the next / previous block in the ring buffer
|
|
|
|
*/
|
|
|
|
*/
|
|
|
|
static FORCE_INLINE int8_t next_block_index(int8_t block_index) { return BLOCK_MOD(block_index + 1); }
|
|
|
|
static int8_t next_block_index(int8_t block_index) { return BLOCK_MOD(block_index + 1); }
|
|
|
|
static FORCE_INLINE int8_t prev_block_index(int8_t block_index) { return BLOCK_MOD(block_index - 1); }
|
|
|
|
static int8_t prev_block_index(int8_t block_index) { return BLOCK_MOD(block_index - 1); }
|
|
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
/**
|
|
|
|
* Calculate the distance (not time) it takes to accelerate
|
|
|
|
* Calculate the distance (not time) it takes to accelerate
|
|
|
|
* from initial_rate to target_rate using the given acceleration:
|
|
|
|
* from initial_rate to target_rate using the given acceleration:
|
|
|
|
*/
|
|
|
|
*/
|
|
|
|
static FORCE_INLINE float estimate_acceleration_distance(float initial_rate, float target_rate, float acceleration) {
|
|
|
|
static float estimate_acceleration_distance(float initial_rate, float target_rate, float acceleration) {
|
|
|
|
if (acceleration == 0) return 0; // acceleration was 0, set acceleration distance to 0
|
|
|
|
if (acceleration == 0) return 0; // acceleration was 0, set acceleration distance to 0
|
|
|
|
return (target_rate * target_rate - initial_rate * initial_rate) / (acceleration * 2);
|
|
|
|
return (target_rate * target_rate - initial_rate * initial_rate) / (acceleration * 2);
|
|
|
|
}
|
|
|
|
}
|
|
|
@ -292,7 +292,7 @@ class Planner {
|
|
|
|
* This is used to compute the intersection point between acceleration and deceleration
|
|
|
|
* This is used to compute the intersection point between acceleration and deceleration
|
|
|
|
* in cases where the "trapezoid" has no plateau (i.e., never reaches maximum speed)
|
|
|
|
* in cases where the "trapezoid" has no plateau (i.e., never reaches maximum speed)
|
|
|
|
*/
|
|
|
|
*/
|
|
|
|
static FORCE_INLINE float intersection_distance(float initial_rate, float final_rate, float acceleration, float distance) {
|
|
|
|
static float intersection_distance(float initial_rate, float final_rate, float acceleration, float distance) {
|
|
|
|
if (acceleration == 0) return 0; // acceleration was 0, set intersection distance to 0
|
|
|
|
if (acceleration == 0) return 0; // acceleration was 0, set intersection distance to 0
|
|
|
|
return (acceleration * 2 * distance - initial_rate * initial_rate + final_rate * final_rate) / (acceleration * 4);
|
|
|
|
return (acceleration * 2 * distance - initial_rate * initial_rate + final_rate * final_rate) / (acceleration * 4);
|
|
|
|
}
|
|
|
|
}
|
|
|
@ -302,7 +302,7 @@ class Planner {
|
|
|
|
* to reach 'target_velocity' using 'acceleration' within a given
|
|
|
|
* to reach 'target_velocity' using 'acceleration' within a given
|
|
|
|
* 'distance'.
|
|
|
|
* 'distance'.
|
|
|
|
*/
|
|
|
|
*/
|
|
|
|
static FORCE_INLINE float max_allowable_speed(float acceleration, float target_velocity, float distance) {
|
|
|
|
static float max_allowable_speed(float acceleration, float target_velocity, float distance) {
|
|
|
|
return sqrt(target_velocity * target_velocity - 2 * acceleration * distance);
|
|
|
|
return sqrt(target_velocity * target_velocity - 2 * acceleration * distance);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|