bissen21/Marlin_Firmware
1
0
Fork 0
Code Issues Pull Requests Projects Releases 4 Wiki Activity

Planner singleton class

Browse Source
This commit is contained in:
Scott Lahteine
2016-04-27 18:06:32 -07:00
parent 5076d12344
commit 96f51f400f
10 changed files with 587 additions and 499 deletions
Download Patch File Download Diff File Expand all files Collapse all files

350
Marlin/planner.cpp
View File

@@ -81,105 +81,27 @@
#include "mesh_bed_leveling.h"
#endif
//===========================================================================
//============================= public variables ============================
//===========================================================================
Planner planner;
millis_t minsegmenttime;
float max_feedrate[NUM_AXIS]; // Max speeds in mm per minute
float axis_steps_per_unit[NUM_AXIS];
unsigned long max_acceleration_units_per_sq_second[NUM_AXIS]; // Use M201 to override by software
float minimumfeedrate;
float acceleration; // Normal acceleration mm/s^2 DEFAULT ACCELERATION for all printing moves. M204 SXXXX
float retract_acceleration; // Retract acceleration mm/s^2 filament pull-back and push-forward while standing still in the other axes M204 TXXXX
float travel_acceleration; // Travel acceleration mm/s^2 DEFAULT ACCELERATION for all NON printing moves. M204 MXXXX
float max_xy_jerk; // The largest speed change requiring no acceleration
float max_z_jerk;
float max_e_jerk;
float mintravelfeedrate;
unsigned long axis_steps_per_sqr_second[NUM_AXIS];
#if ENABLED(AUTO_BED_LEVELING_FEATURE)
// Transform required to compensate for bed level
matrix_3x3 plan_bed_level_matrix = {
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0
};
#endif // AUTO_BED_LEVELING_FEATURE
#if ENABLED(AUTOTEMP)
float autotemp_max = 250;
float autotemp_min = 210;
float autotemp_factor = 0.1;
bool autotemp_enabled = false;
#endif
#if ENABLED(FAN_SOFT_PWM)
extern unsigned char fanSpeedSoftPwm[FAN_COUNT];
#endif
//===========================================================================
//============ semi-private variables, used in inline functions =============
//===========================================================================
block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instfructions
volatile unsigned char block_buffer_head; // Index of the next block to be pushed
volatile unsigned char block_buffer_tail; // Index of the block to process now
//===========================================================================
//============================ private variables ============================
//===========================================================================
// The current position of the tool in absolute steps
long position[NUM_AXIS]; // Rescaled from extern when axis_steps_per_unit are changed by gcode
static float previous_speed[NUM_AXIS]; // Speed of previous path line segment
static float previous_nominal_speed; // Nominal speed of previous path line segment
uint8_t g_uc_extruder_last_move[EXTRUDERS] = { 0 };
#ifdef XY_FREQUENCY_LIMIT
// Used for the frequency limit
#define MAX_FREQ_TIME (1000000.0/XY_FREQUENCY_LIMIT)
// Old direction bits. Used for speed calculations
static unsigned char old_direction_bits = 0;
// Segment times (in µs). Used for speed calculations
static long axis_segment_time[2][3] = { {MAX_FREQ_TIME + 1, 0, 0}, {MAX_FREQ_TIME + 1, 0, 0} };
#endif
#if ENABLED(DUAL_X_CARRIAGE)
extern bool extruder_duplication_enabled;
#endif
//===========================================================================
//================================ functions ================================
//===========================================================================
// Get the next / previous index of the next block in the ring buffer
// NOTE: Using & here (not %) because BLOCK_BUFFER_SIZE is always a power of 2
FORCE_INLINE int8_t next_block_index(int8_t block_index) { return BLOCK_MOD(block_index + 1); }
FORCE_INLINE int8_t prev_block_index(int8_t block_index) { return BLOCK_MOD(block_index - 1); }
// Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the
// given acceleration:
FORCE_INLINE 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
return (target_rate * target_rate - initial_rate * initial_rate) / (acceleration * 2);
Planner::Planner() {
#if ENABLED(AUTO_BED_LEVELING_FEATURE)
bed_level_matrix.set_to_identity();
#endif
init();
}
// This function gives you the point at which you must start braking (at the rate of -acceleration) if
// you started at speed initial_rate and accelerated until this point and want to end at the final_rate after
// a total travel of distance. This can be used to compute the intersection point between acceleration and
// deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed)
FORCE_INLINE 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
return (acceleration * 2 * distance - initial_rate * initial_rate + final_rate * final_rate) / (acceleration * 4);
void Planner::init() {
block_buffer_head = block_buffer_tail = 0;
memset(position, 0, sizeof(position)); // clear position
for (int i = 0; i < NUM_AXIS; i++) previous_speed[i] = 0.0;
previous_nominal_speed = 0.0;
}
// Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors.
void calculate_trapezoid_for_block(block_t* block, float entry_factor, float exit_factor) {
/**
* Calculate trapezoid parameters, multiplying the entry- and exit-speeds
* by the provided factors.
*/
void Planner::calculate_trapezoid_for_block(block_t* block, float entry_factor, float exit_factor) {
unsigned long initial_rate = ceil(block->nominal_rate * entry_factor),
final_rate = ceil(block->nominal_rate * exit_factor); // (steps per second)
@@ -225,12 +147,6 @@ void calculate_trapezoid_for_block(block_t* block, float entry_factor, float exi
CRITICAL_SECTION_END;
}
// Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the
// acceleration within the allotted distance.
FORCE_INLINE float max_allowable_speed(float acceleration, float target_velocity, float distance) {
return sqrt(target_velocity * target_velocity - 2 * acceleration * distance);
}
// "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.
@@ -240,8 +156,8 @@ FORCE_INLINE float max_allowable_speed(float acceleration, float target_velocity
//}
// The kernel called by planner_recalculate() when scanning the plan from last to first entry.
void planner_reverse_pass_kernel(block_t* previous, block_t* current, block_t* next) {
// The kernel called by recalculate() when scanning the plan from last to first entry.
void Planner::reverse_pass_kernel(block_t* previous, block_t* current, block_t* next) {
if (!current) return;
UNUSED(previous);
@@ -267,31 +183,34 @@ void planner_reverse_pass_kernel(block_t* previous, block_t* current, block_t* n
} // Skip last block. Already initialized and set for recalculation.
}
// planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This
// implements the reverse pass.
void planner_reverse_pass() {
uint8_t block_index = block_buffer_head;
/**
* recalculate() needs to go over the current plan twice.
* Once in reverse and once forward. This implements the reverse pass.
*/
void Planner::reverse_pass() {
//Make a local copy of block_buffer_tail, because the interrupt can alter it
CRITICAL_SECTION_START;
unsigned char tail = block_buffer_tail;
CRITICAL_SECTION_END
if (movesplanned() > 3) {
if (BLOCK_MOD(block_buffer_head - tail + BLOCK_BUFFER_SIZE) > 3) { // moves queued
block_index = BLOCK_MOD(block_buffer_head - 3);
block_t* block[3] = { NULL, NULL, NULL };
while (block_index != tail) {
block_index = prev_block_index(block_index);
// Make a local copy of block_buffer_tail, because the interrupt can alter it
CRITICAL_SECTION_START;
uint8_t tail = block_buffer_tail;
CRITICAL_SECTION_END
uint8_t b = BLOCK_MOD(block_buffer_head - 3);
while (b != tail) {
b = prev_block_index(b);
block[2] = block[1];
block[1] = block[0];
block[0] = &block_buffer[block_index];
planner_reverse_pass_kernel(block[0], block[1], block[2]);
block[0] = &block_buffer[b];
reverse_pass_kernel(block[0], block[1], block[2]);
}
}
}
// The kernel called by planner_recalculate() when scanning the plan from first to last entry.
void planner_forward_pass_kernel(block_t* previous, block_t* current, block_t* next) {
// The kernel called by recalculate() when scanning the plan from first to last entry.
void Planner::forward_pass_kernel(block_t* previous, block_t* current, block_t* next) {
if (!previous) return;
UNUSED(next);
@@ -312,26 +231,28 @@ void planner_forward_pass_kernel(block_t* previous, block_t* current, block_t* n
}
}
// planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This
// implements the forward pass.
void planner_forward_pass() {
uint8_t block_index = block_buffer_tail;
/**
* recalculate() needs to go over the current plan twice.
* Once in reverse and once forward. This implements the forward pass.
*/
void Planner::forward_pass() {
block_t* block[3] = { NULL, NULL, NULL };
while (block_index != block_buffer_head) {
for (uint8_t b = block_buffer_tail; b != block_buffer_head; b = next_block_index(b)) {
block[0] = block[1];
block[1] = block[2];
block[2] = &block_buffer[block_index];
planner_forward_pass_kernel(block[0], block[1], block[2]);
block_index = next_block_index(block_index);
block[2] = &block_buffer[b];
forward_pass_kernel(block[0], block[1], block[2]);
}
planner_forward_pass_kernel(block[1], block[2], NULL);
forward_pass_kernel(block[1], block[2], NULL);
}
// Recalculates the trapezoid speed profiles for all blocks in the plan according to the
// entry_factor for each junction. Must be called by planner_recalculate() after
// updating the blocks.
void planner_recalculate_trapezoids() {
/**
* Recalculate the trapezoid speed profiles for all blocks in the plan
* according to the entry_factor for each junction. Must be called by
* recalculate() after updating the blocks.
*/
void Planner::recalculate_trapezoids() {
int8_t block_index = block_buffer_tail;
block_t* current;
block_t* next = NULL;
@@ -358,54 +279,52 @@ void planner_recalculate_trapezoids() {
}
}
// Recalculates the motion plan according to the following algorithm:
//
// 1. Go over every block in reverse order and calculate a junction speed reduction (i.e. block_t.entry_factor)
// so that:
// a. The junction jerk is within the set limit
// b. No speed reduction within one block requires faster deceleration than the one, true constant
// acceleration.
// 2. Go over every block in chronological order and dial down junction speed reduction values if
// a. The speed increase within one block would require faster acceleration than the one, true
// constant acceleration.
//
// When these stages are complete all blocks have an entry_factor that will allow all speed changes to
// be performed using only the one, true constant acceleration, and where no junction jerk is jerkier than
// the set limit. Finally it will:
//
// 3. Recalculate trapezoids for all blocks.
void planner_recalculate() {
planner_reverse_pass();
planner_forward_pass();
planner_recalculate_trapezoids();
}
void plan_init() {
block_buffer_head = block_buffer_tail = 0;
memset(position, 0, sizeof(position)); // clear position
for (int i = 0; i < NUM_AXIS; i++) previous_speed[i] = 0.0;
previous_nominal_speed = 0.0;
/*
* Recalculate the motion plan according to the following algorithm:
*
* 1. Go over every block in reverse order...
*
* Calculate a junction speed reduction (block_t.entry_factor) so:
*
* a. The junction jerk is within the set limit, and
*
* b. No speed reduction within one block requires faster
* deceleration than the one, true constant acceleration.
*
* 2. Go over every block in chronological order...
*
* Dial down junction speed reduction values if:
* a. The speed increase within one block would require faster
* acceleration than the one, true constant acceleration.
*
* After that, all blocks will have an entry_factor allowing all speed changes to
* be performed using only the one, true constant acceleration, and where no junction
* jerk is jerkier than the set limit, Jerky. Finally it will:
*
* 3. Recalculate "trapezoids" for all blocks.
*/
void Planner::recalculate() {
reverse_pass();
forward_pass();
recalculate_trapezoids();
}
#if ENABLED(AUTOTEMP)
void getHighESpeed() {
void Planner::getHighESpeed() {
static float oldt = 0;
if (!autotemp_enabled) return;
if (degTargetHotend0() + 2 < autotemp_min) return; // probably temperature set to zero.
float high = 0.0;
uint8_t block_index = block_buffer_tail;
while (block_index != block_buffer_head) {
block_t* block = &block_buffer[block_index];
for (uint8_t b = block_buffer_tail; b != block_buffer_head; b = next_block_index(b)) {
block_t* block = &block_buffer[b];
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;
NOLESS(high, se);
}
block_index = next_block_index(block_index);
}
float t = autotemp_min + high * autotemp_factor;
@@ -417,9 +336,13 @@ void plan_init() {
oldt = t;
setTargetHotend0(t);
}
#endif //AUTOTEMP
void check_axes_activity() {
/**
* Maintain fans, paste extruder pressure,
*/
void Planner::check_axes_activity() {
unsigned char axis_active[NUM_AXIS] = { 0 },
tail_fan_speed[FAN_COUNT];
@@ -432,26 +355,23 @@ void check_axes_activity() {
tail_e_to_p_pressure = baricuda_e_to_p_pressure;
#endif
block_t* block;
if (blocks_queued()) {
uint8_t block_index = block_buffer_tail;
#if FAN_COUNT > 0
for (uint8_t i = 0; i < FAN_COUNT; i++) tail_fan_speed[i] = block_buffer[block_index].fan_speed[i];
for (uint8_t i = 0; i < FAN_COUNT; i++) tail_fan_speed[i] = block_buffer[block_buffer_tail].fan_speed[i];
#endif
block_t* block;
#if ENABLED(BARICUDA)
block = &block_buffer[block_index];
block = &block_buffer[block_buffer_tail];
tail_valve_pressure = block->valve_pressure;
tail_e_to_p_pressure = block->e_to_p_pressure;
#endif
while (block_index != block_buffer_head) {
block = &block_buffer[block_index];
for (uint8_t b = block_buffer_tail; b != block_buffer_head; b = next_block_index(b)) {
block = &block_buffer[b];
for (int i = 0; i < NUM_AXIS; i++) if (block->steps[i]) axis_active[i]++;
block_index = next_block_index(block_index);
}
}
#if ENABLED(DISABLE_X)
@@ -549,15 +469,20 @@ void check_axes_activity() {
#endif
}
/**
* Planner::buffer_line
*
* Add a new linear movement to the buffer.
*
* x,y,z,e - target position in mm
* feed_rate - (target) speed of the move
* extruder - target extruder
*/
float junction_deviation = 0.1;
// Add a new linear movement to the buffer. steps[X_AXIS], _y and _z is the absolute position in
// mm. Microseconds specify how many microseconds the move should take to perform. To aid acceleration
// calculation the caller must also provide the physical length of the line in millimeters.
#if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING)
void plan_buffer_line(float x, float y, float z, const float& e, float feed_rate, const uint8_t extruder)
void Planner::buffer_line(float x, float y, float z, const float& e, float feed_rate, const uint8_t extruder)
#else
void plan_buffer_line(const float& x, const float& y, const float& z, const float& e, float feed_rate, const uint8_t extruder)
void Planner::buffer_line(const float& x, const float& y, const float& z, const float& e, float feed_rate, const uint8_t extruder)
#endif // AUTO_BED_LEVELING_FEATURE
{
// Calculate the buffer head after we push this byte
@@ -570,7 +495,7 @@ float junction_deviation = 0.1;
#if ENABLED(MESH_BED_LEVELING)
if (mbl.active) z += mbl.get_z(x - home_offset[X_AXIS], y - home_offset[Y_AXIS]);
#elif ENABLED(AUTO_BED_LEVELING_FEATURE)
apply_rotation_xyz(plan_bed_level_matrix, x, y, z);
apply_rotation_xyz(bed_level_matrix, x, y, z);
#endif
// The target position of the tool in absolute steps
@@ -703,7 +628,8 @@ float junction_deviation = 0.1;
// Enable extruder(s)
if (block->steps[E_AXIS]) {
if (DISABLE_INACTIVE_EXTRUDER) { //enable only selected extruder
#if ENABLED(DISABLE_INACTIVE_EXTRUDER) // Enable only the selected extruder
for (int i = 0; i < EXTRUDERS; i++)
if (g_uc_extruder_last_move[i] > 0) g_uc_extruder_last_move[i]--;
@@ -762,19 +688,18 @@ float junction_deviation = 0.1;
#endif // EXTRUDERS > 2
#endif // EXTRUDERS > 1
}
}
else { // enable all
#else
enable_e0();
enable_e1();
enable_e2();
enable_e3();
}
#endif
}
if (block->steps[E_AXIS])
NOLESS(feed_rate, minimumfeedrate);
NOLESS(feed_rate, min_feedrate);
else
NOLESS(feed_rate, mintravelfeedrate);
NOLESS(feed_rate, min_travel_feedrate);
/**
* This part of the code calculates the total length of the movement.
@@ -837,9 +762,9 @@ float junction_deviation = 0.1;
// segment time im micro seconds
unsigned long segment_time = lround(1000000.0/inverse_second);
if (mq) {
if (segment_time < minsegmenttime) {
if (segment_time < min_segment_time) {
// buffer is draining, add extra time. The amount of time added increases if the buffer is still emptied more.
inverse_second = 1000000.0 / (segment_time + lround(2 * (minsegmenttime - segment_time) / moves_queued));
inverse_second = 1000000.0 / (segment_time + lround(2 * (min_segment_time - segment_time) / moves_queued));
#ifdef XY_FREQUENCY_LIMIT
segment_time = lround(1000000.0 / inverse_second);
#endif
@@ -968,6 +893,9 @@ float junction_deviation = 0.1;
block->acceleration_rate = (long)(acc_st * 16777216.0 / (F_CPU / 8.0));
#if 0 // Use old jerk for now
float junction_deviation = 0.1;
// Compute path unit vector
double unit_vec[3];
@@ -1083,11 +1011,11 @@ float junction_deviation = 0.1;
// Update position
for (int i = 0; i < NUM_AXIS; i++) position[i] = target[i];
planner_recalculate();
recalculate();
stepper.wake_up();
} // plan_buffer_line()
} // buffer_line()
#if ENABLED(AUTO_BED_LEVELING_FEATURE) && DISABLED(DELTA)
@@ -1096,13 +1024,15 @@ float junction_deviation = 0.1;
*
* On CORE machines XYZ is derived from ABC.
*/
vector_3 plan_get_position() {
vector_3 Planner::adjusted_position() {
vector_3 position = vector_3(stepper.get_axis_position_mm(X_AXIS), stepper.get_axis_position_mm(Y_AXIS), stepper.get_axis_position_mm(Z_AXIS));
//position.debug("in plan_get position");
//plan_bed_level_matrix.debug("in plan_get_position");
matrix_3x3 inverse = matrix_3x3::transpose(plan_bed_level_matrix);
//inverse.debug("in plan_get inverse");
//position.debug("in Planner::position");
//bed_level_matrix.debug("in Planner::position");
matrix_3x3 inverse = matrix_3x3::transpose(bed_level_matrix);
//inverse.debug("in Planner::inverse");
position.apply_rotation(inverse);
//position.debug("after rotation");
@@ -1117,15 +1047,15 @@ float junction_deviation = 0.1;
* On CORE machines stepper ABC will be translated from the given XYZ.
*/
#if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING)
void plan_set_position(float x, float y, float z, const float& e)
void Planner::set_position(float x, float y, float z, const float& e)
#else
void plan_set_position(const float& x, const float& y, const float& z, const float& e)
void Planner::set_position(const float& x, const float& y, const float& z, const float& e)
#endif // AUTO_BED_LEVELING_FEATURE || MESH_BED_LEVELING
{
#if ENABLED(MESH_BED_LEVELING)
if (mbl.active) z += mbl.get_z(x - home_offset[X_AXIS], y - home_offset[Y_AXIS]);
#elif ENABLED(AUTO_BED_LEVELING_FEATURE)
apply_rotation_xyz(plan_bed_level_matrix, x, y, z);
apply_rotation_xyz(bed_level_matrix, x, y, z);
#endif
long nx = position[X_AXIS] = lround(x * axis_steps_per_unit[X_AXIS]),
@@ -1138,13 +1068,27 @@ float junction_deviation = 0.1;
for (int i = 0; i < NUM_AXIS; i++) previous_speed[i] = 0.0;
}
void plan_set_e_position(const float& e) {
/**
* Directly set the planner E position (hence the stepper E position).
*/
void Planner::set_e_position(const float& e) {
position[E_AXIS] = lround(e * axis_steps_per_unit[E_AXIS]);
stepper.set_e_position(position[E_AXIS]);
}
// Calculate the steps/s^2 acceleration rates, based on the mm/s^s
void reset_acceleration_rates() {
// Recalculate the steps/s^2 acceleration rates, based on the mm/s^2
void Planner::reset_acceleration_rates() {
for (int i = 0; i < NUM_AXIS; i++)
axis_steps_per_sqr_second[i] = max_acceleration_units_per_sq_second[i] * axis_steps_per_unit[i];
}
#if ENABLED(AUTOTEMP)
void Planner::autotemp_M109() {
autotemp_enabled = code_seen('F');
if (autotemp_enabled) autotemp_factor = code_value();
if (code_seen('S')) autotemp_min = code_value();
if (code_seen('B')) autotemp_max = code_value();
}
#endif