forksand
/
marlin-lulzbot-laser
1
1
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge pull request #1658 from thinkyhead/planner_oh_planner

Browse Source 
Cleanup of planner code
master
Scott Lahteine 10 years ago
parent c6df5f4d85 8db4fd7177
commit 512a0056a9
15 changed files with 807 additions and 766 deletions
Show Stats Download Patch File Download Diff File

4
Marlin/Marlin.h
Unescape View File

@ -178,7 +178,7 @@ void manage_inactivity(bool ignore_stepper_queue=false);
#define disable_e3() /* nothing */ #define disable_e3() /* nothing */
#endif #endif
enum AxisEnum {X_AXIS=0, Y_AXIS=1, Z_AXIS=2, E_AXIS=3, X_HEAD=4, Y_HEAD=5}; enum AxisEnum {X_AXIS=0, Y_AXIS=1, A_AXIS=0, B_AXIS=1, Z_AXIS=2, E_AXIS=3, X_HEAD=4, Y_HEAD=5};
//X_HEAD and Y_HEAD is used for systems that don't have a 1:1 relationship between X_AXIS and X Head movement, like CoreXY bots. //X_HEAD and Y_HEAD is used for systems that don't have a 1:1 relationship between X_AXIS and X Head movement, like CoreXY bots.
void FlushSerialRequestResend(); void FlushSerialRequestResend();
@ -265,7 +265,7 @@ extern int fanSpeed;
extern bool filament_sensor; //indicates that filament sensor readings should control extrusion extern bool filament_sensor; //indicates that filament sensor readings should control extrusion
extern float filament_width_meas; //holds the filament diameter as accurately measured extern float filament_width_meas; //holds the filament diameter as accurately measured
extern signed char measurement_delay[]; //ring buffer to delay measurement extern signed char measurement_delay[]; //ring buffer to delay measurement
extern int delay_index1, delay_index2; //index into ring buffer extern int delay_index1, delay_index2; //ring buffer index. used by planner, temperature, and main code
extern float delay_dist; //delay distance counter extern float delay_dist; //delay distance counter
extern int meas_delay_cm; //delay distance extern int meas_delay_cm; //delay distance
#endif #endif

17
Marlin/configurator/config/Configuration.h
Unescape View File

@ -406,6 +406,23 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
//const bool FIL_RUNOUT_INVERTING = true; // Should be uncommented and true or false should assigned //const bool FIL_RUNOUT_INVERTING = true; // Should be uncommented and true or false should assigned
//#define ENDSTOPPULLUP_FIL_RUNOUT // Uncomment to use internal pullup for filament runout pins if the sensor is defined. //#define ENDSTOPPULLUP_FIL_RUNOUT // Uncomment to use internal pullup for filament runout pins if the sensor is defined.
//===========================================================================
//============================ Manual Bed Leveling ==========================
//===========================================================================
// #define MANUAL_BED_LEVELING // Add display menu option for bed leveling
// #define MESH_BED_LEVELING // Enable mesh bed leveling
#if defined(MESH_BED_LEVELING)
#define MESH_MIN_X 10
#define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
#define MESH_MIN_Y 10
#define MESH_MAX_Y (Y_MAX_POS - MESH_MIN_Y)
#define MESH_NUM_X_POINTS 3 // Don't use more than 7 points per axis, implementation limited
#define MESH_NUM_Y_POINTS 3
#define MESH_HOME_SEARCH_Z 4 // Z after Home, bed somewhere below but above 0.0
#endif // MESH_BED_LEVELING
//=========================================================================== //===========================================================================
//============================= Bed Auto Leveling =========================== //============================= Bed Auto Leveling ===========================
//=========================================================================== //===========================================================================

17
Marlin/example_configurations/Felix/Configuration.h
Unescape View File

@ -354,6 +354,23 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
//const bool FIL_RUNOUT_INVERTING = true; // Should be uncommented and true or false should assigned //const bool FIL_RUNOUT_INVERTING = true; // Should be uncommented and true or false should assigned
//#define ENDSTOPPULLUP_FIL_RUNOUT // Uncomment to use internal pullup for filament runout pins if the sensor is defined. //#define ENDSTOPPULLUP_FIL_RUNOUT // Uncomment to use internal pullup for filament runout pins if the sensor is defined.
//===========================================================================
//============================ Manual Bed Leveling ==========================
//===========================================================================
// #define MANUAL_BED_LEVELING // Add display menu option for bed leveling
// #define MESH_BED_LEVELING // Enable mesh bed leveling
#if defined(MESH_BED_LEVELING)
#define MESH_MIN_X 10
#define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
#define MESH_MIN_Y 10
#define MESH_MAX_Y (Y_MAX_POS - MESH_MIN_Y)
#define MESH_NUM_X_POINTS 3 // Don't use more than 7 points per axis, implementation limited
#define MESH_NUM_Y_POINTS 3
#define MESH_HOME_SEARCH_Z 4 // Z after Home, bed somewhere below but above 0.0
#endif // MESH_BED_LEVELING
//=========================================================================== //===========================================================================
//============================= Bed Auto Leveling =========================== //============================= Bed Auto Leveling ===========================
//=========================================================================== //===========================================================================

17
Marlin/example_configurations/Felix/Configuration_DUAL.h
Unescape View File

@ -354,6 +354,23 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
//const bool FIL_RUNOUT_INVERTING = true; // Should be uncommented and true or false should assigned //const bool FIL_RUNOUT_INVERTING = true; // Should be uncommented and true or false should assigned
//#define ENDSTOPPULLUP_FIL_RUNOUT // Uncomment to use internal pullup for filament runout pins if the sensor is defined. //#define ENDSTOPPULLUP_FIL_RUNOUT // Uncomment to use internal pullup for filament runout pins if the sensor is defined.
//===========================================================================
//============================ Manual Bed Leveling ==========================
//===========================================================================
// #define MANUAL_BED_LEVELING // Add display menu option for bed leveling
// #define MESH_BED_LEVELING // Enable mesh bed leveling
#if defined(MESH_BED_LEVELING)
#define MESH_MIN_X 10
#define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
#define MESH_MIN_Y 10
#define MESH_MAX_Y (Y_MAX_POS - MESH_MIN_Y)
#define MESH_NUM_X_POINTS 3 // Don't use more than 7 points per axis, implementation limited
#define MESH_NUM_Y_POINTS 3
#define MESH_HOME_SEARCH_Z 4 // Z after Home, bed somewhere below but above 0.0
#endif // MESH_BED_LEVELING
//=========================================================================== //===========================================================================
//============================= Bed Auto Leveling =========================== //============================= Bed Auto Leveling ===========================
//=========================================================================== //===========================================================================

17
Marlin/example_configurations/Hephestos/Configuration.h
Unescape View File

@ -380,6 +380,23 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
//const bool FIL_RUNOUT_INVERTING = true; // Should be uncommented and true or false should assigned //const bool FIL_RUNOUT_INVERTING = true; // Should be uncommented and true or false should assigned
//#define ENDSTOPPULLUP_FIL_RUNOUT // Uncomment to use internal pullup for filament runout pins if the sensor is defined. //#define ENDSTOPPULLUP_FIL_RUNOUT // Uncomment to use internal pullup for filament runout pins if the sensor is defined.
//===========================================================================
//============================ Manual Bed Leveling ==========================
//===========================================================================
// #define MANUAL_BED_LEVELING // Add display menu option for bed leveling
// #define MESH_BED_LEVELING // Enable mesh bed leveling
#if defined(MESH_BED_LEVELING)
#define MESH_MIN_X 10
#define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
#define MESH_MIN_Y 10
#define MESH_MAX_Y (Y_MAX_POS - MESH_MIN_Y)
#define MESH_NUM_X_POINTS 3 // Don't use more than 7 points per axis, implementation limited
#define MESH_NUM_Y_POINTS 3
#define MESH_HOME_SEARCH_Z 4 // Z after Home, bed somewhere below but above 0.0
#endif // MESH_BED_LEVELING
//=========================================================================== //===========================================================================
//============================= Bed Auto Leveling =========================== //============================= Bed Auto Leveling ===========================
//=========================================================================== //===========================================================================

17
Marlin/example_configurations/K8200/Configuration.h
Unescape View File

@ -385,6 +385,23 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
//const bool FIL_RUNOUT_INVERTING = true; // Should be uncommented and true or false should assigned //const bool FIL_RUNOUT_INVERTING = true; // Should be uncommented and true or false should assigned
//#define ENDSTOPPULLUP_FIL_RUNOUT // Uncomment to use internal pullup for filament runout pins if the sensor is defined. //#define ENDSTOPPULLUP_FIL_RUNOUT // Uncomment to use internal pullup for filament runout pins if the sensor is defined.
//===========================================================================
//============================ Manual Bed Leveling ==========================
//===========================================================================
// #define MANUAL_BED_LEVELING // Add display menu option for bed leveling
// #define MESH_BED_LEVELING // Enable mesh bed leveling
#if defined(MESH_BED_LEVELING)
#define MESH_MIN_X 10
#define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
#define MESH_MIN_Y 10
#define MESH_MAX_Y (Y_MAX_POS - MESH_MIN_Y)
#define MESH_NUM_X_POINTS 3 // Don't use more than 7 points per axis, implementation limited
#define MESH_NUM_Y_POINTS 3
#define MESH_HOME_SEARCH_Z 4 // Z after Home, bed somewhere below but above 0.0
#endif // MESH_BED_LEVELING
//=========================================================================== //===========================================================================
//============================= Bed Auto Leveling =========================== //============================= Bed Auto Leveling ===========================
//=========================================================================== //===========================================================================

17
Marlin/example_configurations/SCARA/Configuration.h
Unescape View File

@ -409,6 +409,23 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
//const bool FIL_RUNOUT_INVERTING = true; // Should be uncommented and true or false should assigned //const bool FIL_RUNOUT_INVERTING = true; // Should be uncommented and true or false should assigned
//#define ENDSTOPPULLUP_FIL_RUNOUT // Uncomment to use internal pullup for filament runout pins if the sensor is defined. //#define ENDSTOPPULLUP_FIL_RUNOUT // Uncomment to use internal pullup for filament runout pins if the sensor is defined.
//===========================================================================
//============================ Manual Bed Leveling ==========================
//===========================================================================
// #define MANUAL_BED_LEVELING // Add display menu option for bed leveling
// #define MESH_BED_LEVELING // Enable mesh bed leveling
#if defined(MESH_BED_LEVELING)
#define MESH_MIN_X 10
#define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
#define MESH_MIN_Y 10
#define MESH_MAX_Y (Y_MAX_POS - MESH_MIN_Y)
#define MESH_NUM_X_POINTS 3 // Don't use more than 7 points per axis, implementation limited
#define MESH_NUM_Y_POINTS 3
#define MESH_HOME_SEARCH_Z 4 // Z after Home, bed somewhere below but above 0.0
#endif // MESH_BED_LEVELING
//=========================================================================== //===========================================================================
//============================= Bed Auto Leveling =========================== //============================= Bed Auto Leveling ===========================
//=========================================================================== //===========================================================================

17
Marlin/example_configurations/WITBOX/Configuration.h
Unescape View File

@ -379,6 +379,23 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
//const bool FIL_RUNOUT_INVERTING = true; // Should be uncommented and true or false should assigned //const bool FIL_RUNOUT_INVERTING = true; // Should be uncommented and true or false should assigned
//#define ENDSTOPPULLUP_FIL_RUNOUT // Uncomment to use internal pullup for filament runout pins if the sensor is defined. //#define ENDSTOPPULLUP_FIL_RUNOUT // Uncomment to use internal pullup for filament runout pins if the sensor is defined.
//===========================================================================
//============================ Manual Bed Leveling ==========================
//===========================================================================
// #define MANUAL_BED_LEVELING // Add display menu option for bed leveling
// #define MESH_BED_LEVELING // Enable mesh bed leveling
#if defined(MESH_BED_LEVELING)
#define MESH_MIN_X 10
#define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
#define MESH_MIN_Y 10
#define MESH_MAX_Y (Y_MAX_POS - MESH_MIN_Y)
#define MESH_NUM_X_POINTS 3 // Don't use more than 7 points per axis, implementation limited
#define MESH_NUM_Y_POINTS 3
#define MESH_HOME_SEARCH_Z 4 // Z after Home, bed somewhere below but above 0.0
#endif // MESH_BED_LEVELING
//=========================================================================== //===========================================================================
//============================= Bed Auto Leveling =========================== //============================= Bed Auto Leveling ===========================
//=========================================================================== //===========================================================================

17
Marlin/example_configurations/delta/generic/Configuration.h
Unescape View File

@ -409,6 +409,23 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
//const bool FIL_RUNOUT_INVERTING = true; // Should be uncommented and true or false should assigned //const bool FIL_RUNOUT_INVERTING = true; // Should be uncommented and true or false should assigned
//#define ENDSTOPPULLUP_FIL_RUNOUT // Uncomment to use internal pullup for filament runout pins if the sensor is defined. //#define ENDSTOPPULLUP_FIL_RUNOUT // Uncomment to use internal pullup for filament runout pins if the sensor is defined.
//===========================================================================
//============================ Manual Bed Leveling ==========================
//===========================================================================
// #define MANUAL_BED_LEVELING // Add display menu option for bed leveling
// #define MESH_BED_LEVELING // Enable mesh bed leveling
#if defined(MESH_BED_LEVELING)
#define MESH_MIN_X 10
#define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
#define MESH_MIN_Y 10
#define MESH_MAX_Y (Y_MAX_POS - MESH_MIN_Y)
#define MESH_NUM_X_POINTS 3 // Don't use more than 7 points per axis, implementation limited
#define MESH_NUM_Y_POINTS 3
#define MESH_HOME_SEARCH_Z 4 // Z after Home, bed somewhere below but above 0.0
#endif // MESH_BED_LEVELING
//=========================================================================== //===========================================================================
//============================= Bed Auto Leveling =========================== //============================= Bed Auto Leveling ===========================
//=========================================================================== //===========================================================================

17
Marlin/example_configurations/delta/kossel_mini/Configuration.h
Unescape View File

@ -410,6 +410,23 @@ const bool Z_MAX_ENDSTOP_INVERTING = false; // set to true to invert the logic o
//const bool FIL_RUNOUT_INVERTING = true; // Should be uncommented and true or false should assigned //const bool FIL_RUNOUT_INVERTING = true; // Should be uncommented and true or false should assigned
//#define ENDSTOPPULLUP_FIL_RUNOUT // Uncomment to use internal pullup for filament runout pins if the sensor is defined. //#define ENDSTOPPULLUP_FIL_RUNOUT // Uncomment to use internal pullup for filament runout pins if the sensor is defined.
//===========================================================================
//============================ Manual Bed Leveling ==========================
//===========================================================================
// #define MANUAL_BED_LEVELING // Add display menu option for bed leveling
// #define MESH_BED_LEVELING // Enable mesh bed leveling
#if defined(MESH_BED_LEVELING)
#define MESH_MIN_X 10
#define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
#define MESH_MIN_Y 10
#define MESH_MAX_Y (Y_MAX_POS - MESH_MIN_Y)
#define MESH_NUM_X_POINTS 3 // Don't use more than 7 points per axis, implementation limited
#define MESH_NUM_Y_POINTS 3
#define MESH_HOME_SEARCH_Z 4 // Z after Home, bed somewhere below but above 0.0
#endif // MESH_BED_LEVELING
//=========================================================================== //===========================================================================
//============================= Bed Auto Leveling =========================== //============================= Bed Auto Leveling ===========================
//=========================================================================== //===========================================================================

17
Marlin/example_configurations/makibox/Configuration.h
Unescape View File

@ -377,6 +377,23 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
//const bool FIL_RUNOUT_INVERTING = true; // Should be uncommented and true or false should assigned //const bool FIL_RUNOUT_INVERTING = true; // Should be uncommented and true or false should assigned
//#define ENDSTOPPULLUP_FIL_RUNOUT // Uncomment to use internal pullup for filament runout pins if the sensor is defined. //#define ENDSTOPPULLUP_FIL_RUNOUT // Uncomment to use internal pullup for filament runout pins if the sensor is defined.
//===========================================================================
//============================ Manual Bed Leveling ==========================
//===========================================================================
// #define MANUAL_BED_LEVELING // Add display menu option for bed leveling
// #define MESH_BED_LEVELING // Enable mesh bed leveling
#if defined(MESH_BED_LEVELING)
#define MESH_MIN_X 10
#define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
#define MESH_MIN_Y 10
#define MESH_MAX_Y (Y_MAX_POS - MESH_MIN_Y)
#define MESH_NUM_X_POINTS 3 // Don't use more than 7 points per axis, implementation limited
#define MESH_NUM_Y_POINTS 3
#define MESH_HOME_SEARCH_Z 4 // Z after Home, bed somewhere below but above 0.0
#endif // MESH_BED_LEVELING
//=========================================================================== //===========================================================================
//============================= Bed Auto Leveling =========================== //============================= Bed Auto Leveling ===========================
//=========================================================================== //===========================================================================

17
Marlin/example_configurations/tvrrug/Round2/Configuration.h
Unescape View File

@ -379,6 +379,23 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
//const bool FIL_RUNOUT_INVERTING = true; // Should be uncommented and true or false should assigned //const bool FIL_RUNOUT_INVERTING = true; // Should be uncommented and true or false should assigned
//#define ENDSTOPPULLUP_FIL_RUNOUT // Uncomment to use internal pullup for filament runout pins if the sensor is defined. //#define ENDSTOPPULLUP_FIL_RUNOUT // Uncomment to use internal pullup for filament runout pins if the sensor is defined.
//===========================================================================
//============================ Manual Bed Leveling ==========================
//===========================================================================
// #define MANUAL_BED_LEVELING // Add display menu option for bed leveling
// #define MESH_BED_LEVELING // Enable mesh bed leveling
#if defined(MESH_BED_LEVELING)
#define MESH_MIN_X 10
#define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
#define MESH_MIN_Y 10
#define MESH_MAX_Y (Y_MAX_POS - MESH_MIN_Y)
#define MESH_NUM_X_POINTS 3 // Don't use more than 7 points per axis, implementation limited
#define MESH_NUM_Y_POINTS 3
#define MESH_HOME_SEARCH_Z 4 // Z after Home, bed somewhere below but above 0.0
#endif // MESH_BED_LEVELING
//=========================================================================== //===========================================================================
//============================= Bed Auto Leveling =========================== //============================= Bed Auto Leveling ===========================
//=========================================================================== //===========================================================================

693
Marlin/planner.cpp
Unescape View File

@ -117,52 +117,32 @@ volatile unsigned char block_buffer_tail; // Index of the block to pro
float extrude_min_temp = EXTRUDE_MINTEMP; float extrude_min_temp = EXTRUDE_MINTEMP;
#endif #endif
#ifdef XY_FREQUENCY_LIMIT #ifdef XY_FREQUENCY_LIMIT
#define MAX_FREQ_TIME (1000000.0/XY_FREQUENCY_LIMIT)
// Used for the frequency limit // Used for the frequency limit
static unsigned char old_direction_bits = 0; // Old direction bits. Used for speed calculations #define MAX_FREQ_TIME (1000000.0/XY_FREQUENCY_LIMIT)
static long x_segment_time[3]={MAX_FREQ_TIME + 1,0,0}; // Segment times (in us). Used for speed calculations // Old direction bits. Used for speed calculations
static long y_segment_time[3]={MAX_FREQ_TIME + 1,0,0}; 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 #endif
#ifdef FILAMENT_SENSOR #ifdef FILAMENT_SENSOR
static char meas_sample; //temporary variable to hold filament measurement sample static char meas_sample; //temporary variable to hold filament measurement sample
#endif #endif
// Returns the index of the next block in the ring buffer // Get the next / previous index of the next block in the ring buffer
// NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication. // NOTE: Using & here (not %) because BLOCK_BUFFER_SIZE is always a power of 2
static int8_t next_block_index(int8_t block_index) { FORCE_INLINE int8_t next_block_index(int8_t block_index) { return BLOCK_MOD(block_index + 1); }
block_index++; FORCE_INLINE int8_t prev_block_index(int8_t block_index) { return BLOCK_MOD(block_index - 1); }
if (block_index == BLOCK_BUFFER_SIZE) {
block_index = 0;
}
return(block_index);
}
// Returns the index of the previous block in the ring buffer
static int8_t prev_block_index(int8_t block_index) {
if (block_index == 0) {
block_index = BLOCK_BUFFER_SIZE;
}
block_index--;
return(block_index);
}
//=========================================================================== //===========================================================================
//=============================functions ============================ //================================ Functions ================================
//=========================================================================== //===========================================================================
// Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the // Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the
// given acceleration: // given acceleration:
FORCE_INLINE float estimate_acceleration_distance(float initial_rate, float target_rate, float 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
if (acceleration!=0) { return (target_rate * target_rate - initial_rate * initial_rate) / (acceleration * 2);
return((target_rate*target_rate-initial_rate*initial_rate)/
(2.0*acceleration));
}
else {
return 0.0; // acceleration was 0, set acceleration distance to 0
}
} }
// This function gives you the point at which you must start braking (at the rate of -acceleration) if // This function gives you the point at which you must start braking (at the rate of -acceleration) if
@ -170,15 +150,9 @@ FORCE_INLINE float estimate_acceleration_distance(float initial_rate, float targ
// a total travel of distance. This can be used to compute the intersection point between acceleration and // 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) // 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) 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
if (acceleration!=0) { return (acceleration * 2 * distance - initial_rate * initial_rate + final_rate * final_rate) / (acceleration * 4);
return((2.0*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/
(4.0*acceleration) );
}
else {
return 0.0; // acceleration was 0, set intersection distance to 0
}
} }
// Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors. // Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors.
@ -188,18 +162,12 @@ void calculate_trapezoid_for_block(block_t *block, float entry_factor, float exi
unsigned long final_rate = ceil(block->nominal_rate * exit_factor); // (step/min) unsigned long final_rate = ceil(block->nominal_rate * exit_factor); // (step/min)
// Limit minimal step rate (Otherwise the timer will overflow.) // Limit minimal step rate (Otherwise the timer will overflow.)
if(initial_rate <120) { if (initial_rate < 120) initial_rate = 120;
initial_rate=120; if (final_rate < 120) final_rate = 120;
}
if(final_rate < 120) {
final_rate=120;
}
long acceleration = block->acceleration_st; long acceleration = block->acceleration_st;
int32_t accelerate_steps = int32_t accelerate_steps = ceil(estimate_acceleration_distance(initial_rate, block->nominal_rate, acceleration));
ceil(estimate_acceleration_distance(initial_rate, block->nominal_rate, acceleration)); int32_t decelerate_steps = floor(estimate_acceleration_distance(block->nominal_rate, final_rate, -acceleration));
int32_t decelerate_steps =
floor(estimate_acceleration_distance(block->nominal_rate, final_rate, -acceleration));
// Calculate the size of Plateau of Nominal Rate. // Calculate the size of Plateau of Nominal Rate.
int32_t plateau_steps = block->step_event_count - accelerate_steps - decelerate_steps; int32_t plateau_steps = block->step_event_count - accelerate_steps - decelerate_steps;
@ -222,7 +190,7 @@ void calculate_trapezoid_for_block(block_t *block, float entry_factor, float exi
// block->accelerate_until = accelerate_steps; // block->accelerate_until = accelerate_steps;
// block->decelerate_after = accelerate_steps+plateau_steps; // block->decelerate_after = accelerate_steps+plateau_steps;
CRITICAL_SECTION_START; // Fill variables used by the stepper in a critical section CRITICAL_SECTION_START; // Fill variables used by the stepper in a critical section
if(block->busy == false) { // Don't update variables if block is busy. if (!block->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;
block->initial_rate = initial_rate; block->initial_rate = initial_rate;
@ -230,7 +198,7 @@ void calculate_trapezoid_for_block(block_t *block, float entry_factor, float exi
#ifdef ADVANCE #ifdef ADVANCE
block->initial_advance = initial_advance; block->initial_advance = initial_advance;
block->final_advance = final_advance; block->final_advance = final_advance;
#endif //ADVANCE #endif
} }
CRITICAL_SECTION_END; CRITICAL_SECTION_END;
} }
@ -252,9 +220,7 @@ 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. // 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) { void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *next) {
if(!current) { if (!current) return;
return;
}
if (next) { if (next) {
// 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, no need to recheck. Block is cruising.
@ -264,7 +230,7 @@ void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *n
// If nominal length true, max junction speed is guaranteed to be reached. Only compute // 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. // for max allowable speed if block is decelerating and nominal length is false.
if ((!current->nominal_length_flag) && (current->max_entry_speed > next->entry_speed)) { if (!current->nominal_length_flag && current->max_entry_speed > next->entry_speed) {
current->entry_speed = min(current->max_entry_speed, current->entry_speed = min(current->max_entry_speed,
max_allowable_speed(-current->acceleration, next->entry_speed, current->millimeters)); max_allowable_speed(-current->acceleration, next->entry_speed, current->millimeters));
} }
@ -287,10 +253,9 @@ void planner_reverse_pass() {
unsigned char tail = block_buffer_tail; unsigned char tail = block_buffer_tail;
CRITICAL_SECTION_END CRITICAL_SECTION_END
if(((block_buffer_head-tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1)) > 3) { if (BLOCK_MOD(block_buffer_head - tail + BLOCK_BUFFER_SIZE) > 3) { // moves queued
block_index = (block_buffer_head - 3) & (BLOCK_BUFFER_SIZE - 1); block_index = BLOCK_MOD(block_buffer_head - 3);
block_t *block[3] = { block_t *block[3] = { NULL, NULL, NULL };
NULL, NULL, NULL };
while (block_index != tail) { while (block_index != tail) {
block_index = prev_block_index(block_index); block_index = prev_block_index(block_index);
block[2]= block[1]; block[2]= block[1];
@ -303,9 +268,7 @@ void planner_reverse_pass() {
// The kernel called by planner_recalculate() when scanning the plan from first to last entry. // 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) { void planner_forward_pass_kernel(block_t *previous, block_t *current, block_t *next) {
if(!previous) { if (!previous) return;
return;
}
// If the previous block is an acceleration block, but it is not long enough to complete the // If the previous block is an acceleration block, but it is not long enough to complete the
// full speed change within the block, we need to adjust the entry speed accordingly. Entry // full speed change within the block, we need to adjust the entry speed accordingly. Entry
@ -329,8 +292,7 @@ void planner_forward_pass_kernel(block_t *previous, block_t *current, block_t *n
// implements the forward pass. // implements the forward pass.
void planner_forward_pass() { void planner_forward_pass() {
uint8_t block_index = block_buffer_tail; uint8_t block_index = block_buffer_tail;
block_t *block[3] = { block_t *block[3] = { NULL, NULL, NULL };
NULL, NULL, NULL };
while (block_index != block_buffer_head) { while (block_index != block_buffer_head) {
block[0] = block[1]; block[0] = block[1];
@ -357,17 +319,17 @@ void planner_recalculate_trapezoids() {
// Recalculate if current block entry or exit junction speed has changed. // Recalculate if current block entry or exit junction speed has changed.
if (current->recalculate_flag || next->recalculate_flag) { if (current->recalculate_flag || next->recalculate_flag) {
// NOTE: Entry and exit factors always > 0 by all previous logic operations. // NOTE: Entry and exit factors always > 0 by all previous logic operations.
calculate_trapezoid_for_block(current, current->entry_speed/current->nominal_speed, float nom = current->nominal_speed;
next->entry_speed/current->nominal_speed); calculate_trapezoid_for_block(current, current->entry_speed / nom, next->entry_speed / nom);
current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed
} }
} }
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 != NULL) { if (next) {
calculate_trapezoid_for_block(next, next->entry_speed/next->nominal_speed, float nom = next->nominal_speed;
MINIMUM_PLANNER_SPEED/next->nominal_speed); calculate_trapezoid_for_block(next, next->entry_speed / nom, MINIMUM_PLANNER_SPEED / nom);
next->recalculate_flag = false; next->recalculate_flag = false;
} }
} }
@ -396,104 +358,76 @@ void planner_recalculate() {
} }
void plan_init() { void plan_init() {
block_buffer_head = 0; block_buffer_head = block_buffer_tail = 0;
block_buffer_tail = 0;
memset(position, 0, sizeof(position)); // clear position memset(position, 0, sizeof(position)); // clear position
previous_speed[0] = 0.0; for (int i=0; i<NUM_AXIS; i++) previous_speed[i] = 0.0;
previous_speed[1] = 0.0;
previous_speed[2] = 0.0;
previous_speed[3] = 0.0;
previous_nominal_speed = 0.0; previous_nominal_speed = 0.0;
} }
#ifdef AUTOTEMP #ifdef AUTOTEMP
void getHighESpeed() void getHighESpeed() {
{
static float oldt = 0; static float oldt = 0;
if(!autotemp_enabled){
return; if (!autotemp_enabled) return;
} if (degTargetHotend0() + 2 < autotemp_min) return; // probably temperature set to zero.
if(degTargetHotend0()+2<autotemp_min) { //probably temperature set to zero.
return; //do nothing
}
float high = 0.0; float high = 0.0;
uint8_t block_index = block_buffer_tail; uint8_t block_index = block_buffer_tail;
while (block_index != block_buffer_head) { while (block_index != block_buffer_head) {
if((block_buffer[block_index].steps_x != 0) || block_t *block = &block_buffer[block_index];
(block_buffer[block_index].steps_y != 0) || if (block->steps[X_AXIS] || block->steps[Y_AXIS] || block->steps[Z_AXIS]) {
(block_buffer[block_index].steps_z != 0)) { float se = (float)block->steps[E_AXIS] / block->step_event_count * block->nominal_speed; // mm/sec;
float se=(float(block_buffer[block_index].steps_e)/float(block_buffer[block_index].step_event_count))*block_buffer[block_index].nominal_speed; if (se > high) high = se;
//se; mm/sec;
if(se>high)
{
high=se;
}
} }
block_index = (block_index+1) & (BLOCK_BUFFER_SIZE - 1); block_index = next_block_index(block_index);
} }
float g=autotemp_min+high*autotemp_factor; float t = autotemp_min + high * autotemp_factor;
float t=g; if (t < autotemp_min) t = autotemp_min;
if(t<autotemp_min) if (t > autotemp_max) t = autotemp_max;
t=autotemp_min; if (oldt > t) t = AUTOTEMP_OLDWEIGHT * oldt + (1 - AUTOTEMP_OLDWEIGHT) * t;
if(t>autotemp_max)
t=autotemp_max;
if(oldt>t)
{
t=AUTOTEMP_OLDWEIGHT*oldt+(1-AUTOTEMP_OLDWEIGHT)*t;
}
oldt = t; oldt = t;
setTargetHotend0(t); setTargetHotend0(t);
} }
#endif #endif
void check_axes_activity() void check_axes_activity() {
{ unsigned char axis_active[NUM_AXIS],
unsigned char x_active = 0; tail_fan_speed = fanSpeed;
unsigned char y_active = 0;
unsigned char z_active = 0;
unsigned char e_active = 0;
unsigned char tail_fan_speed = fanSpeed;
#ifdef BARICUDA #ifdef BARICUDA
unsigned char tail_valve_pressure = ValvePressure; unsigned char tail_valve_pressure = ValvePressure,
unsigned char tail_e_to_p_pressure = EtoPPressure; tail_e_to_p_pressure = EtoPPressure;
#endif #endif
block_t *block; block_t *block;
if(block_buffer_tail != block_buffer_head) if (blocks_queued()) {
{
uint8_t block_index = block_buffer_tail; uint8_t block_index = block_buffer_tail;
tail_fan_speed = block_buffer[block_index].fan_speed; tail_fan_speed = block_buffer[block_index].fan_speed;
#ifdef BARICUDA #ifdef BARICUDA
tail_valve_pressure = block_buffer[block_index].valve_pressure; block = &block_buffer[block_index];
tail_e_to_p_pressure = block_buffer[block_index].e_to_p_pressure; tail_valve_pressure = block->valve_pressure;
tail_e_to_p_pressure = block->e_to_p_pressure;
#endif #endif
while(block_index != block_buffer_head) while (block_index != block_buffer_head) {
{
block = &block_buffer[block_index]; block = &block_buffer[block_index];
if(block->steps_x != 0) x_active++; for (int i=0; i<NUM_AXIS; i++) if (block->steps[i]) axis_active[i]++;
if(block->steps_y != 0) y_active++; block_index = next_block_index(block_index);
if(block->steps_z != 0) z_active++;
if(block->steps_e != 0) e_active++;
block_index = (block_index+1) & (BLOCK_BUFFER_SIZE - 1);
} }
} }
if((DISABLE_X) && (x_active == 0)) disable_x(); if (DISABLE_X && !axis_active[X_AXIS]) disable_x();
if((DISABLE_Y) && (y_active == 0)) disable_y(); if (DISABLE_Y && !axis_active[Y_AXIS]) disable_y();
if((DISABLE_Z) && (z_active == 0)) disable_z(); if (DISABLE_Z && !axis_active[Z_AXIS]) disable_z();
if((DISABLE_E) && (e_active == 0)) if (DISABLE_E && !axis_active[E_AXIS]) {
{
disable_e0(); disable_e0();
disable_e1(); disable_e1();
disable_e2(); disable_e2();
disable_e3(); disable_e3();
} }
#if defined(FAN_PIN) && FAN_PIN > -1
#if defined(FAN_PIN) && FAN_PIN > -1 // HAS_FAN
#ifdef FAN_KICKSTART_TIME #ifdef FAN_KICKSTART_TIME
static unsigned long fan_kick_end; static unsigned long fan_kick_end;
if (tail_fan_speed) { if (tail_fan_speed) {
@ -514,16 +448,16 @@ void check_axes_activity()
analogWrite(FAN_PIN, tail_fan_speed); analogWrite(FAN_PIN, tail_fan_speed);
#endif //!FAN_SOFT_PWM #endif //!FAN_SOFT_PWM
#endif //FAN_PIN > -1 #endif //FAN_PIN > -1
#ifdef AUTOTEMP #ifdef AUTOTEMP
getHighESpeed(); getHighESpeed();
#endif #endif
#ifdef BARICUDA #ifdef BARICUDA
#if defined(HEATER_1_PIN) && HEATER_1_PIN > -1 #if defined(HEATER_1_PIN) && HEATER_1_PIN > -1 // HAS_HEATER_1
analogWrite(HEATER_1_PIN,tail_valve_pressure); analogWrite(HEATER_1_PIN,tail_valve_pressure);
#endif #endif
#if defined(HEATER_2_PIN) && HEATER_2_PIN > -1 // HAS_HEATER_2
#if defined(HEATER_2_PIN) && HEATER_2_PIN > -1
analogWrite(HEATER_2_PIN,tail_e_to_p_pressure); analogWrite(HEATER_2_PIN,tail_e_to_p_pressure);
#endif #endif
#endif #endif
@ -531,7 +465,7 @@ void check_axes_activity()
float junction_deviation = 0.1; float junction_deviation = 0.1;
// Add a new linear movement to the buffer. steps_x, _y and _z is the absolute position in // 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 // 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. // calculation the caller must also provide the physical length of the line in millimeters.
#if defined(ENABLE_AUTO_BED_LEVELING) || defined(MESH_BED_LEVELING) #if defined(ENABLE_AUTO_BED_LEVELING) || defined(MESH_BED_LEVELING)
@ -545,46 +479,44 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
// If the buffer is full: good! That means we are well ahead of the robot. // If the buffer is full: good! That means we are well ahead of the robot.
// Rest here until there is room in the buffer. // Rest here until there is room in the buffer.
while(block_buffer_tail == next_buffer_head) while(block_buffer_tail == next_buffer_head) {
{
manage_heater(); manage_heater();
manage_inactivity(); manage_inactivity();
lcd_update(); lcd_update();
} }
#if defined(MESH_BED_LEVELING) #ifdef MESH_BED_LEVELING
if (mbl.active) { if (mbl.active) z += mbl.get_z(x, y);
z += mbl.get_z(x, y); #endif
}
#endif // MESH_BED_LEVELING
#ifdef ENABLE_AUTO_BED_LEVELING #ifdef ENABLE_AUTO_BED_LEVELING
apply_rotation_xyz(plan_bed_level_matrix, x, y, z); apply_rotation_xyz(plan_bed_level_matrix, x, y, z);
#endif // ENABLE_AUTO_BED_LEVELING #endif
// The target position of the tool in absolute steps // The target position of the tool in absolute steps
// Calculate target position in absolute steps // Calculate target position in absolute steps
//this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow //this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
long target[4]; long target[NUM_AXIS];
target[X_AXIS] = lround(x * axis_steps_per_unit[X_AXIS]); target[X_AXIS] = lround(x * axis_steps_per_unit[X_AXIS]);
target[Y_AXIS] = lround(y * axis_steps_per_unit[Y_AXIS]); target[Y_AXIS] = lround(y * axis_steps_per_unit[Y_AXIS]);
target[Z_AXIS] = lround(z * axis_steps_per_unit[Z_AXIS]); target[Z_AXIS] = lround(z * axis_steps_per_unit[Z_AXIS]);
target[E_AXIS] = lround(e * axis_steps_per_unit[E_AXIS]); target[E_AXIS] = lround(e * axis_steps_per_unit[E_AXIS]);
float dx = target[X_AXIS] - position[X_AXIS],
dy = target[Y_AXIS] - position[Y_AXIS],
dz = target[Z_AXIS] - position[Z_AXIS],
de = target[E_AXIS] - position[E_AXIS];
#ifdef PREVENT_DANGEROUS_EXTRUDE #ifdef PREVENT_DANGEROUS_EXTRUDE
if(target[E_AXIS]!=position[E_AXIS]) if (de) {
{ if (degHotend(active_extruder) < extrude_min_temp) {
if(degHotend(active_extruder)<extrude_min_temp)
{
position[E_AXIS] = target[E_AXIS]; //behave as if the move really took place, but ignore E part position[E_AXIS] = target[E_AXIS]; //behave as if the move really took place, but ignore E part
SERIAL_ECHO_START; SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP); SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP);
} }
#ifdef PREVENT_LENGTHY_EXTRUDE #ifdef PREVENT_LENGTHY_EXTRUDE
if(labs(target[E_AXIS]-position[E_AXIS])>axis_steps_per_unit[E_AXIS]*EXTRUDE_MAXLENGTH) if (labs(de) > axis_steps_per_unit[E_AXIS] * EXTRUDE_MAXLENGTH) {
{ position[E_AXIS] = target[E_AXIS]; // Behave as if the move really took place, but ignore E part
position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part
SERIAL_ECHO_START; SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP); SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP);
} }
@ -599,28 +531,26 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
block->busy = false; block->busy = false;
// Number of steps for each axis // Number of steps for each axis
#ifndef COREXY #ifdef COREXY
// default non-h-bot planning
block->steps_x = labs(target[X_AXIS]-position[X_AXIS]);
block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]);
#else
// corexy planning // corexy planning
// these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html // these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html
block->steps_x = labs((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS])); block->steps[A_AXIS] = labs(dx + dy);
block->steps_y = labs((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS])); block->steps[B_AXIS] = labs(dx - dy);
#else
// default non-h-bot planning
block->steps[X_AXIS] = labs(dx);
block->steps[Y_AXIS] = labs(dy);
#endif #endif
block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]);
block->steps_e = labs(target[E_AXIS]-position[E_AXIS]); block->steps[Z_AXIS] = labs(dz);
block->steps_e *= volumetric_multiplier[active_extruder]; block->steps[E_AXIS] = labs(de);
block->steps_e *= extrudemultiply; block->steps[E_AXIS] *= volumetric_multiplier[active_extruder];
block->steps_e /= 100; block->steps[E_AXIS] *= extrudemultiply;
block->step_event_count = max(block->steps_x, max(block->steps_y, max(block->steps_z, block->steps_e))); block->steps[E_AXIS] /= 100;
block->step_event_count = max(block->steps[X_AXIS], max(block->steps[Y_AXIS], max(block->steps[Z_AXIS], block->steps[E_AXIS])));
// Bail if this is a zero-length block // Bail if this is a zero-length block
if (block->step_event_count <= dropsegments) if (block->step_event_count <= dropsegments) return;
{
return;
}
block->fan_speed = fanSpeed; block->fan_speed = fanSpeed;
#ifdef BARICUDA #ifdef BARICUDA
@ -629,109 +559,94 @@ block->steps_y = labs((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-positi
#endif #endif
// Compute direction bits for this block // Compute direction bits for this block
block->direction_bits = 0; uint8_t db = 0;
#ifndef COREXY #ifdef COREXY
if (target[X_AXIS] < position[X_AXIS]) if (dx < 0) db |= BIT(X_HEAD); // Save the real Extruder (head) direction in X Axis
{ if (dy < 0) db |= BIT(Y_HEAD); // ...and Y
block->direction_bits |= BIT(X_AXIS); if (dx + dy < 0) db |= BIT(A_AXIS); // Motor A direction
} if (dx - dy < 0) db |= BIT(B_AXIS); // Motor B direction
if (target[Y_AXIS] < position[Y_AXIS])
{
block->direction_bits |= BIT(Y_AXIS);
}
#else #else
if (target[X_AXIS] < position[X_AXIS]) if (dx < 0) db |= BIT(X_AXIS);
{ if (dy < 0) db |= BIT(Y_AXIS);
block->direction_bits |= BIT(X_HEAD); //AlexBorro: Save the real Extruder (head) direction in X Axis
}
if (target[Y_AXIS] < position[Y_AXIS])
{
block->direction_bits |= BIT(Y_HEAD); //AlexBorro: Save the real Extruder (head) direction in Y Axis
}
if ((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]) < 0)
{
block->direction_bits |= BIT(X_AXIS); //AlexBorro: Motor A direction (Incorrectly implemented as X_AXIS)
}
if ((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]) < 0)
{
block->direction_bits |= BIT(Y_AXIS); //AlexBorro: Motor B direction (Incorrectly implemented as Y_AXIS)
}
#endif #endif
if (target[Z_AXIS] < position[Z_AXIS]) if (dz < 0) db |= BIT(Z_AXIS);
{ if (de < 0) db |= BIT(E_AXIS);
block->direction_bits |= BIT(Z_AXIS); block->direction_bits = db;
}
if (target[E_AXIS] < position[E_AXIS])
{
block->direction_bits |= BIT(E_AXIS);
}
block->active_extruder = extruder; block->active_extruder = extruder;
//enable active axes //enable active axes
#ifdef COREXY #ifdef COREXY
if((block->steps_x != 0) || (block->steps_y != 0)) if (block->steps[A_AXIS] || block->steps[B_AXIS]) {
{
enable_x(); enable_x();
enable_y(); enable_y();
} }
#else #else
if(block->steps_x != 0) enable_x(); if (block->steps[X_AXIS]) enable_x();
if(block->steps_y != 0) enable_y(); if (block->steps[Y_AXIS]) enable_y();
#endif #endif
#ifndef Z_LATE_ENABLE #ifndef Z_LATE_ENABLE
if(block->steps_z != 0) enable_z(); if (block->steps[Z_AXIS]) enable_z();
#endif #endif
// Enable extruder(s) // Enable extruder(s)
if(block->steps_e != 0) if (block->steps[E_AXIS]) {
{ if (DISABLE_INACTIVE_EXTRUDER) { //enable only selected extruder
if (DISABLE_INACTIVE_EXTRUDER) //enable only selected extruder
{
if(g_uc_extruder_last_move[0] > 0) g_uc_extruder_last_move[0]--; for (int i=0; i<EXTRUDERS; i++)
if(g_uc_extruder_last_move[1] > 0) g_uc_extruder_last_move[1]--; if (g_uc_extruder_last_move[i] > 0) g_uc_extruder_last_move[i]--;
if(g_uc_extruder_last_move[2] > 0) g_uc_extruder_last_move[2]--;
if(g_uc_extruder_last_move[3] > 0) g_uc_extruder_last_move[3]--;
switch(extruder) switch(extruder) {
{
case 0: case 0:
enable_e0(); enable_e0();
g_uc_extruder_last_move[0] = BLOCK_BUFFER_SIZE * 2; g_uc_extruder_last_move[0] = BLOCK_BUFFER_SIZE * 2;
#if EXTRUDERS > 1
if (g_uc_extruder_last_move[1] == 0) disable_e1(); if (g_uc_extruder_last_move[1] == 0) disable_e1();
#if EXTRUDERS > 2
if (g_uc_extruder_last_move[2] == 0) disable_e2(); if (g_uc_extruder_last_move[2] == 0) disable_e2();
#if EXTRUDERS > 3
if (g_uc_extruder_last_move[3] == 0) disable_e3(); if (g_uc_extruder_last_move[3] == 0) disable_e3();
#endif
#endif
#endif
break; break;
#if EXTRUDERS > 1
case 1: case 1:
enable_e1(); enable_e1();
g_uc_extruder_last_move[1] = BLOCK_BUFFER_SIZE*2; g_uc_extruder_last_move[1] = BLOCK_BUFFER_SIZE*2;
if (g_uc_extruder_last_move[0] == 0) disable_e0(); if (g_uc_extruder_last_move[0] == 0) disable_e0();
#if EXTRUDERS > 2
if (g_uc_extruder_last_move[2] == 0) disable_e2(); if (g_uc_extruder_last_move[2] == 0) disable_e2();
#if EXTRUDERS > 3
if (g_uc_extruder_last_move[3] == 0) disable_e3(); if (g_uc_extruder_last_move[3] == 0) disable_e3();
#endif
#endif
break; break;
#if EXTRUDERS > 2
case 2: case 2:
enable_e2(); enable_e2();
g_uc_extruder_last_move[2] = BLOCK_BUFFER_SIZE*2; g_uc_extruder_last_move[2] = BLOCK_BUFFER_SIZE*2;
if (g_uc_extruder_last_move[0] == 0) disable_e0(); if (g_uc_extruder_last_move[0] == 0) disable_e0();
if (g_uc_extruder_last_move[1] == 0) disable_e1(); if (g_uc_extruder_last_move[1] == 0) disable_e1();
#if EXTRUDERS > 3
if (g_uc_extruder_last_move[3] == 0) disable_e3(); if (g_uc_extruder_last_move[3] == 0) disable_e3();
#endif
break; break;
#if EXTRUDERS > 3
case 3: case 3:
enable_e3(); enable_e3();
g_uc_extruder_last_move[3] = BLOCK_BUFFER_SIZE*2; g_uc_extruder_last_move[3] = BLOCK_BUFFER_SIZE*2;
if (g_uc_extruder_last_move[0] == 0) disable_e0(); if (g_uc_extruder_last_move[0] == 0) disable_e0();
if (g_uc_extruder_last_move[1] == 0) disable_e1(); if (g_uc_extruder_last_move[1] == 0) disable_e1();
if (g_uc_extruder_last_move[2] == 0) disable_e2(); if (g_uc_extruder_last_move[2] == 0) disable_e2();
break; break;
#endif // EXTRUDERS > 3
#endif // EXTRUDERS > 2
#endif // EXTRUDERS > 1
} }
} }
else //enable all else { // enable all
{
enable_e0(); enable_e0();
enable_e1(); enable_e1();
enable_e2(); enable_e2();
@ -739,67 +654,65 @@ block->steps_y = labs((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-positi
} }
} }
if (block->steps_e == 0) if (block->steps[E_AXIS]) {
{
if(feed_rate<mintravelfeedrate) feed_rate=mintravelfeedrate;
}
else
{
if (feed_rate < minimumfeedrate) feed_rate = minimumfeedrate; if (feed_rate < minimumfeedrate) feed_rate = minimumfeedrate;
} }
else if (feed_rate < mintravelfeedrate) feed_rate = mintravelfeedrate;
/* This part of the code calculates the total length of the movement. /**
For cartesian bots, the X_AXIS is the real X movement and same for Y_AXIS. * This part of the code calculates the total length of the movement.
But for corexy bots, that is not true. The "X_AXIS" and "Y_AXIS" motors (that should be named to A_AXIS * For cartesian bots, the X_AXIS is the real X movement and same for Y_AXIS.
and B_AXIS) cannot be used for X and Y length, because A=X+Y and B=X-Y. * But for corexy bots, that is not true. The "X_AXIS" and "Y_AXIS" motors (that should be named to A_AXIS
So we need to create other 2 "AXIS", named X_HEAD and Y_HEAD, meaning the real displacement of the Head. * and B_AXIS) cannot be used for X and Y length, because A=X+Y and B=X-Y.
Having the real displacement of the head, we can calculate the total movement length and apply the desired speed. * So we need to create other 2 "AXIS", named X_HEAD and Y_HEAD, meaning the real displacement of the Head.
* Having the real displacement of the head, we can calculate the total movement length and apply the desired speed.
*/ */
#ifndef COREXY #ifdef COREXY
float delta_mm[4];
delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/axis_steps_per_unit[X_AXIS];
delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS];
#else
float delta_mm[6]; float delta_mm[6];
delta_mm[X_HEAD] = (target[X_AXIS]-position[X_AXIS])/axis_steps_per_unit[X_AXIS]; delta_mm[X_HEAD] = dx / axis_steps_per_unit[A_AXIS];
delta_mm[Y_HEAD] = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS]; delta_mm[Y_HEAD] = dy / axis_steps_per_unit[B_AXIS];
delta_mm[X_AXIS] = ((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]))/axis_steps_per_unit[X_AXIS]; delta_mm[A_AXIS] = (dx + dy) / axis_steps_per_unit[A_AXIS];
delta_mm[Y_AXIS] = ((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]))/axis_steps_per_unit[Y_AXIS]; delta_mm[B_AXIS] = (dx - dy) / axis_steps_per_unit[B_AXIS];
#else
float delta_mm[4];
delta_mm[X_AXIS] = dx / axis_steps_per_unit[X_AXIS];
delta_mm[Y_AXIS] = dy / axis_steps_per_unit[Y_AXIS];
#endif #endif
delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/axis_steps_per_unit[Z_AXIS]; delta_mm[Z_AXIS] = dz / axis_steps_per_unit[Z_AXIS];
delta_mm[E_AXIS] = ((target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS])*volumetric_multiplier[active_extruder]*extrudemultiply/100.0; delta_mm[E_AXIS] = (de / axis_steps_per_unit[E_AXIS]) * volumetric_multiplier[active_extruder] * extrudemultiply / 100.0;
if ( block->steps_x <=dropsegments && block->steps_y <=dropsegments && block->steps_z <=dropsegments )
{ if (block->steps[X_AXIS] <= dropsegments && block->steps[Y_AXIS] <= dropsegments && block->steps[Z_AXIS] <= dropsegments) {
block->millimeters = fabs(delta_mm[E_AXIS]); block->millimeters = fabs(delta_mm[E_AXIS]);
} }
else else {
{ block->millimeters = sqrt(
#ifndef COREXY #ifdef COREXY
block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS])); square(delta_mm[X_HEAD]) + square(delta_mm[Y_HEAD])
#else #else
block->millimeters = sqrt(square(delta_mm[X_HEAD]) + square(delta_mm[Y_HEAD]) + square(delta_mm[Z_AXIS])); square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS])
#endif #endif
+ square(delta_mm[Z_AXIS])
);
} }
float inverse_millimeters = 1.0 / block->millimeters; // Inverse millimeters to remove multiple divides float inverse_millimeters = 1.0 / block->millimeters; // Inverse millimeters to remove multiple divides
// Calculate speed in mm/second for each axis. No divide by zero due to previous checks. // Calculate speed in mm/second for each axis. No divide by zero due to previous checks.
float inverse_second = feed_rate * inverse_millimeters; float inverse_second = feed_rate * inverse_millimeters;
int moves_queued=(block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1); int moves_queued = movesplanned();
// slow down when de buffer starts to empty, rather than wait at the corner for a buffer refill // slow down when de buffer starts to empty, rather than wait at the corner for a buffer refill
bool mq = moves_queued > 1 && moves_queued < BLOCK_BUFFER_SIZE / 2;
#ifdef OLD_SLOWDOWN #ifdef OLD_SLOWDOWN
if(moves_queued < (BLOCK_BUFFER_SIZE * 0.5) && moves_queued > 1) if (mq) feed_rate *= 2.0 * moves_queued / BLOCK_BUFFER_SIZE;
feed_rate = feed_rate*moves_queued / (BLOCK_BUFFER_SIZE * 0.5);
#endif #endif
#ifdef SLOWDOWN #ifdef SLOWDOWN
// segment time im micro seconds // segment time im micro seconds
unsigned long segment_time = lround(1000000.0/inverse_second); unsigned long segment_time = lround(1000000.0/inverse_second);
if ((moves_queued > 1) && (moves_queued < (BLOCK_BUFFER_SIZE * 0.5))) if (mq) {
{ if (segment_time < minsegmenttime) {
if (segment_time < minsegmenttime) // buffer is draining, add extra time. The amount of time added increases if the buffer is still emptied more.
{ // 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 * (minsegmenttime - segment_time) / moves_queued));
#ifdef XY_FREQUENCY_LIMIT #ifdef XY_FREQUENCY_LIMIT
segment_time = lround(1000000.0 / inverse_second); segment_time = lround(1000000.0 / inverse_second);
@ -809,135 +722,116 @@ Having the real displacement of the head, we can calculate the total movement le
#endif #endif
// END OF SLOW DOWN SECTION // END OF SLOW DOWN SECTION
block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0 block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0
block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0 block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0
#ifdef FILAMENT_SENSOR #ifdef FILAMENT_SENSOR
//FMM update ring buffer used for delay with filament measurements //FMM update ring buffer used for delay with filament measurements
if (extruder == FILAMENT_SENSOR_EXTRUDER_NUM && delay_index2 > -1) { //only for extruder with filament sensor and if ring buffer is initialized
if((extruder==FILAMENT_SENSOR_EXTRUDER_NUM) && (delay_index2 > -1)) //only for extruder with filament sensor and if ring buffer is initialized const int MMD = MAX_MEASUREMENT_DELAY + 1, MMD10 = MMD * 10;
{
delay_dist = delay_dist + delta_mm[E_AXIS]; //increment counter with next move in e axis
while (delay_dist >= (10*(MAX_MEASUREMENT_DELAY+1))) //check if counter is over max buffer size in mm delay_dist += delta_mm[E_AXIS]; // increment counter with next move in e axis
delay_dist = delay_dist - 10*(MAX_MEASUREMENT_DELAY+1); //loop around the buffer while (delay_dist >= MMD10) delay_dist -= MMD10; // loop around the buffer
while (delay_dist<0) while (delay_dist < 0) delay_dist += MMD10;
delay_dist = delay_dist + 10*(MAX_MEASUREMENT_DELAY+1); //loop around the buffer
delay_index1 = delay_dist / 10.0; // calculate index delay_index1 = delay_dist / 10.0; // calculate index
delay_index1 = constrain(delay_index1, 0, MAX_MEASUREMENT_DELAY); // (already constrained above)
//ensure the number is within range of the array after converting from floating point
if(delay_index1<0) if (delay_index1 != delay_index2) { // moved index
delay_index1=0; meas_sample = widthFil_to_size_ratio() - 100; // Subtract 100 to reduce magnitude - to store in a signed char
else if (delay_index1>MAX_MEASUREMENT_DELAY) while (delay_index1 != delay_index2) {
delay_index1=MAX_MEASUREMENT_DELAY; // Increment and loop around buffer
if (++delay_index2 >= MMD) delay_index2 -= MMD;
if(delay_index1 != delay_index2) //moved index delay_index2 = constrain(delay_index2, 0, MAX_MEASUREMENT_DELAY);
{
meas_sample=widthFil_to_size_ratio()-100; //subtract off 100 to reduce magnitude - to store in a signed char
}
while( delay_index1 != delay_index2)
{
delay_index2 = delay_index2 + 1;
if(delay_index2>MAX_MEASUREMENT_DELAY)
delay_index2=delay_index2-(MAX_MEASUREMENT_DELAY+1); //loop around buffer when incrementing
if(delay_index2<0)
delay_index2=0;
else if (delay_index2>MAX_MEASUREMENT_DELAY)
delay_index2=MAX_MEASUREMENT_DELAY;
measurement_delay[delay_index2] = meas_sample; measurement_delay[delay_index2] = meas_sample;
} }
}
} }
#endif #endif
// Calculate and limit speed in mm/sec for each axis // Calculate and limit speed in mm/sec for each axis
float current_speed[4]; float current_speed[NUM_AXIS];
float speed_factor = 1.0; //factor <=1 do decrease speed float speed_factor = 1.0; //factor <=1 do decrease speed
for(int i=0; i < 4; i++) for (int i = 0; i < NUM_AXIS; i++) {
{
current_speed[i] = delta_mm[i] * inverse_second; current_speed[i] = delta_mm[i] * inverse_second;
if(fabs(current_speed[i]) > max_feedrate[i]) float cs = fabs(current_speed[i]), mf = max_feedrate[i];
speed_factor = min(speed_factor, max_feedrate[i] / fabs(current_speed[i])); if (cs > mf) speed_factor = min(speed_factor, mf / cs);
} }
// Max segement time in us. // Max segement time in us.
#ifdef XY_FREQUENCY_LIMIT #ifdef XY_FREQUENCY_LIMIT
#define MAX_FREQ_TIME (1000000.0 / XY_FREQUENCY_LIMIT) #define MAX_FREQ_TIME (1000000.0 / XY_FREQUENCY_LIMIT)
// Check and limit the xy direction change frequency // Check and limit the xy direction change frequency
unsigned char direction_change = block->direction_bits ^ old_direction_bits; unsigned char direction_change = block->direction_bits ^ old_direction_bits;
old_direction_bits = block->direction_bits; old_direction_bits = block->direction_bits;
segment_time = lround((float)segment_time / speed_factor); segment_time = lround((float)segment_time / speed_factor);
if((direction_change & BIT(X_AXIS)) == 0) long xs0 = axis_segment_time[X_AXIS][0],
{ xs1 = axis_segment_time[X_AXIS][1],
x_segment_time[0] += segment_time; xs2 = axis_segment_time[X_AXIS][2],
ys0 = axis_segment_time[Y_AXIS][0],
ys1 = axis_segment_time[Y_AXIS][1],
ys2 = axis_segment_time[Y_AXIS][2];
if ((direction_change & BIT(X_AXIS)) != 0) {
xs2 = axis_segment_time[X_AXIS][2] = xs1;
xs1 = axis_segment_time[X_AXIS][1] = xs0;
xs0 = 0;
} }
else xs0 = axis_segment_time[X_AXIS][0] = xs0 + segment_time;
{
x_segment_time[2] = x_segment_time[1]; if ((direction_change & BIT(Y_AXIS)) != 0) {
x_segment_time[1] = x_segment_time[0]; ys2 = axis_segment_time[Y_AXIS][2] = axis_segment_time[Y_AXIS][1];
x_segment_time[0] = segment_time; ys1 = axis_segment_time[Y_AXIS][1] = axis_segment_time[Y_AXIS][0];
ys0 = 0;
} }
if((direction_change & BIT(Y_AXIS)) == 0) ys0 = axis_segment_time[Y_AXIS][0] = ys0 + segment_time;
{
y_segment_time[0] += segment_time; long max_x_segment_time = max(xs0, max(xs1, xs2)),
max_y_segment_time = max(ys0, max(ys1, ys2)),
min_xy_segment_time = min(max_x_segment_time, max_y_segment_time);
if (min_xy_segment_time < MAX_FREQ_TIME) {
float low_sf = speed_factor * min_xy_segment_time / MAX_FREQ_TIME;
speed_factor = min(speed_factor, low_sf);
} }
else
{
y_segment_time[2] = y_segment_time[1];
y_segment_time[1] = y_segment_time[0];
y_segment_time[0] = segment_time;
}
long max_x_segment_time = max(x_segment_time[0], max(x_segment_time[1], x_segment_time[2]));
long max_y_segment_time = max(y_segment_time[0], max(y_segment_time[1], y_segment_time[2]));
long min_xy_segment_time =min(max_x_segment_time, max_y_segment_time);
if(min_xy_segment_time < MAX_FREQ_TIME)
speed_factor = min(speed_factor, speed_factor * (float)min_xy_segment_time / (float)MAX_FREQ_TIME);
#endif // XY_FREQUENCY_LIMIT #endif // XY_FREQUENCY_LIMIT
// Correct the speed // Correct the speed
if( speed_factor < 1.0) if (speed_factor < 1.0) {
{ for (unsigned char i = 0; i < NUM_AXIS; i++) current_speed[i] *= speed_factor;
for(unsigned char i=0; i < 4; i++)
{
current_speed[i] *= speed_factor;
}
block->nominal_speed *= speed_factor; block->nominal_speed *= speed_factor;
block->nominal_rate *= speed_factor; block->nominal_rate *= speed_factor;
} }
// Compute and limit the acceleration rate for the trapezoid generator. // Compute and limit the acceleration rate for the trapezoid generator.
float steps_per_mm = block->step_event_count / block->millimeters; float steps_per_mm = block->step_event_count / block->millimeters;
if(block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0) long bsx = block->steps[X_AXIS], bsy = block->steps[Y_AXIS], bsz = block->steps[Z_AXIS], bse = block->steps[E_AXIS];
{ if (bsx == 0 && bsy == 0 && bsz == 0) {
block->acceleration_st = ceil(retract_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2 block->acceleration_st = ceil(retract_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
} }
else if(block->steps_e == 0) else if (bse == 0) {
{
block->acceleration_st = ceil(travel_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2 block->acceleration_st = ceil(travel_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
} }
else else {
{
block->acceleration_st = ceil(acceleration * steps_per_mm); // convert to: acceleration steps/sec^2 block->acceleration_st = ceil(acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
} }
// Limit acceleration per axis // Limit acceleration per axis
if(((float)block->acceleration_st * (float)block->steps_x / (float)block->step_event_count) > axis_steps_per_sqr_second[X_AXIS]) unsigned long acc_st = block->acceleration_st,
block->acceleration_st = axis_steps_per_sqr_second[X_AXIS]; xsteps = axis_steps_per_sqr_second[X_AXIS],
if(((float)block->acceleration_st * (float)block->steps_y / (float)block->step_event_count) > axis_steps_per_sqr_second[Y_AXIS]) ysteps = axis_steps_per_sqr_second[Y_AXIS],
block->acceleration_st = axis_steps_per_sqr_second[Y_AXIS]; zsteps = axis_steps_per_sqr_second[Z_AXIS],
if(((float)block->acceleration_st * (float)block->steps_e / (float)block->step_event_count) > axis_steps_per_sqr_second[E_AXIS]) esteps = axis_steps_per_sqr_second[E_AXIS];
block->acceleration_st = axis_steps_per_sqr_second[E_AXIS]; if ((float)acc_st * bsx / block->step_event_count > xsteps) acc_st = xsteps;
if(((float)block->acceleration_st * (float)block->steps_z / (float)block->step_event_count ) > axis_steps_per_sqr_second[Z_AXIS]) if ((float)acc_st * bsy / block->step_event_count > ysteps) acc_st = ysteps;
block->acceleration_st = axis_steps_per_sqr_second[Z_AXIS]; if ((float)acc_st * bsz / block->step_event_count > zsteps) acc_st = zsteps;
if ((float)acc_st * bse / block->step_event_count > esteps) acc_st = esteps;
block->acceleration = block->acceleration_st / steps_per_mm;
block->acceleration_rate = (long)((float)block->acceleration_st * (16777216.0 / (F_CPU / 8.0))); block->acceleration_st = acc_st;
block->acceleration = acc_st / steps_per_mm;
block->acceleration_rate = (long)(acc_st * 16777216.0 / (F_CPU / 8.0));
#if 0 // Use old jerk for now #if 0 // Use old jerk for now
// Compute path unit vector // Compute path unit vector
@ -979,30 +873,31 @@ Having the real displacement of the head, we can calculate the total movement le
} }
} }
#endif #endif
// Start with a safe speed // Start with a safe speed
float vmax_junction = max_xy_jerk / 2; float vmax_junction = max_xy_jerk / 2;
float vmax_junction_factor = 1.0; float vmax_junction_factor = 1.0;
if(fabs(current_speed[Z_AXIS]) > max_z_jerk/2) float mz2 = max_z_jerk / 2, me2 = max_e_jerk / 2;
vmax_junction = min(vmax_junction, max_z_jerk/2); float csz = current_speed[Z_AXIS], cse = current_speed[E_AXIS];
if(fabs(current_speed[E_AXIS]) > max_e_jerk/2) if (fabs(csz) > mz2) vmax_junction = min(vmax_junction, mz2);
vmax_junction = min(vmax_junction, max_e_jerk/2); if (fabs(cse) > me2) vmax_junction = min(vmax_junction, me2);
vmax_junction = min(vmax_junction, block->nominal_speed); vmax_junction = min(vmax_junction, block->nominal_speed);
float safe_speed = vmax_junction; float safe_speed = vmax_junction;
if ((moves_queued > 1) && (previous_nominal_speed > 0.0001)) { if ((moves_queued > 1) && (previous_nominal_speed > 0.0001)) {
float jerk = sqrt(pow((current_speed[X_AXIS]-previous_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]-previous_speed[Y_AXIS]), 2)); float dx = current_speed[X_AXIS] - previous_speed[X_AXIS],
dy = current_speed[Y_AXIS] - previous_speed[Y_AXIS],
dz = fabs(csz - previous_speed[Z_AXIS]),
de = fabs(cse - previous_speed[E_AXIS]),
jerk = sqrt(dx * dx + dy * dy);
// if ((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) { // if ((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) {
vmax_junction = block->nominal_speed; vmax_junction = block->nominal_speed;
// } // }
if (jerk > max_xy_jerk) { if (jerk > max_xy_jerk) vmax_junction_factor = max_xy_jerk / jerk;
vmax_junction_factor = (max_xy_jerk/jerk); if (dz > max_z_jerk) vmax_junction_factor = min(vmax_junction_factor, max_z_jerk / dz);
} if (de > max_e_jerk) vmax_junction_factor = min(vmax_junction_factor, max_e_jerk / de);
if(fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]) > max_z_jerk) {
vmax_junction_factor= min(vmax_junction_factor, (max_z_jerk/fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS])));
}
if(fabs(current_speed[E_AXIS] - previous_speed[E_AXIS]) > max_e_jerk) {
vmax_junction_factor = min(vmax_junction_factor, (max_e_jerk/fabs(current_speed[E_AXIS] - previous_speed[E_AXIS])));
}
vmax_junction = min(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed vmax_junction = min(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed
} }
block->max_entry_speed = vmax_junction; block->max_entry_speed = vmax_junction;
@ -1019,36 +914,24 @@ Having the real displacement of the head, we can calculate the total movement le
// 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.
if (block->nominal_speed <= v_allowable) { block->nominal_length_flag = (block->nominal_speed <= v_allowable);
block->nominal_length_flag = true;
}
else {
block->nominal_length_flag = false;
}
block->recalculate_flag = true; // Always calculate trapezoid for new block block->recalculate_flag = true; // Always calculate trapezoid for new block
// Update previous path unit_vector and nominal speed // Update previous path unit_vector and nominal speed
memcpy(previous_speed, current_speed, sizeof(previous_speed)); // previous_speed[] = current_speed[] for (int i = 0; i < NUM_AXIS; i++) previous_speed[i] = current_speed[i];
previous_nominal_speed = block->nominal_speed; previous_nominal_speed = block->nominal_speed;
#ifdef ADVANCE #ifdef ADVANCE
// Calculate advance rate // Calculate advance rate
if((block->steps_e == 0) || (block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0)) { if (!bse || (!bsx && !bsy && !bsz)) {
block->advance_rate = 0; block->advance_rate = 0;
block->advance = 0; block->advance = 0;
} }
else { else {
long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_st); long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_st);
float advance = (STEPS_PER_CUBIC_MM_E * EXTRUDER_ADVANCE_K) * float advance = (STEPS_PER_CUBIC_MM_E * EXTRUDER_ADVANCE_K) * (cse * cse * EXTRUSION_AREA * EXTRUSION_AREA) * 256;
(current_speed[E_AXIS] * current_speed[E_AXIS] * EXTRUSION_AREA * EXTRUSION_AREA)*256;
block->advance = advance; block->advance = advance;
if(acc_dist == 0) { block->advance_rate = acc_dist ? advance / (float)acc_dist : 0;
block->advance_rate = 0;
}
else {
block->advance_rate = advance / (float)acc_dist;
}
} }
/* /*
SERIAL_ECHO_START; SERIAL_ECHO_START;
@ -1059,21 +942,21 @@ Having the real displacement of the head, we can calculate the total movement le
*/ */
#endif // ADVANCE #endif // ADVANCE
calculate_trapezoid_for_block(block, block->entry_speed/block->nominal_speed, calculate_trapezoid_for_block(block, block->entry_speed / block->nominal_speed, safe_speed / block->nominal_speed);
safe_speed/block->nominal_speed);
// Move buffer head // Move buffer head
block_buffer_head = next_buffer_head; block_buffer_head = next_buffer_head;
// Update position // Update position
memcpy(position, target, sizeof(target)); // position[] = target[] for (int i = 0; i < NUM_AXIS; i++) position[i] = target[i];
planner_recalculate(); planner_recalculate();
st_wake_up(); st_wake_up();
}
#if defined(ENABLE_AUTO_BED_LEVELING) && not defined(DELTA) } // plan_buffer_line()
#if defined(ENABLE_AUTO_BED_LEVELING) && !defined(DELTA)
vector_3 plan_get_position() { vector_3 plan_get_position() {
vector_3 position = vector_3(st_get_position_mm(X_AXIS), st_get_position_mm(Y_AXIS), st_get_position_mm(Z_AXIS)); vector_3 position = vector_3(st_get_position_mm(X_AXIS), st_get_position_mm(Y_AXIS), st_get_position_mm(Z_AXIS));
@ -1086,7 +969,7 @@ vector_3 plan_get_position() {
return position; return position;
} }
#endif // ENABLE_AUTO_BED_LEVELING #endif // ENABLE_AUTO_BED_LEVELING && !DELTA
#if defined(ENABLE_AUTO_BED_LEVELING) || defined(MESH_BED_LEVELING) #if defined(ENABLE_AUTO_BED_LEVELING) || defined(MESH_BED_LEVELING)
void plan_set_position(float x, float y, float z, const float &e) void plan_set_position(float x, float y, float z, const float &e)
@ -1094,49 +977,33 @@ void plan_set_position(float x, float y, float z, const float &e)
void plan_set_position(const float &x, const float &y, const float &z, const float &e) void plan_set_position(const float &x, const float &y, const float &z, const float &e)
#endif // ENABLE_AUTO_BED_LEVELING || MESH_BED_LEVELING #endif // ENABLE_AUTO_BED_LEVELING || MESH_BED_LEVELING
{ {
#if defined(ENABLE_AUTO_BED_LEVELING) #ifdef ENABLE_AUTO_BED_LEVELING
apply_rotation_xyz(plan_bed_level_matrix, x, y, z); apply_rotation_xyz(plan_bed_level_matrix, x, y, z);
#elif defined(MESH_BED_LEVELING) #elif defined(MESH_BED_LEVELING)
if (mbl.active) { if (mbl.active) z += mbl.get_z(x, y);
z += mbl.get_z(x, y); #endif
}
#endif // ENABLE_AUTO_BED_LEVELING
position[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]); float nx = position[X_AXIS] = lround(x * axis_steps_per_unit[X_AXIS]);
position[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]); float ny = position[Y_AXIS] = lround(y * axis_steps_per_unit[Y_AXIS]);
position[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]); float nz = position[Z_AXIS] = lround(z * axis_steps_per_unit[Z_AXIS]);
position[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]); float ne = position[E_AXIS] = lround(e * axis_steps_per_unit[E_AXIS]);
st_set_position(position[X_AXIS], position[Y_AXIS], position[Z_AXIS], position[E_AXIS]); st_set_position(nx, ny, nz, ne);
previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest. previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.
previous_speed[0] = 0.0;
previous_speed[1] = 0.0; for (int i=0; i<NUM_AXIS; i++) previous_speed[i] = 0.0;
previous_speed[2] = 0.0;
previous_speed[3] = 0.0;
} }
void plan_set_e_position(const float &e) void plan_set_e_position(const float &e) {
{
position[E_AXIS] = lround(e * axis_steps_per_unit[E_AXIS]); position[E_AXIS] = lround(e * axis_steps_per_unit[E_AXIS]);
st_set_e_position(position[E_AXIS]); st_set_e_position(position[E_AXIS]);
} }
uint8_t movesplanned()
{
return (block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1);
}
#ifdef PREVENT_DANGEROUS_EXTRUDE #ifdef PREVENT_DANGEROUS_EXTRUDE
void set_extrude_min_temp(float temp) void set_extrude_min_temp(float temp) { extrude_min_temp = temp; }
{
extrude_min_temp=temp;
}
#endif #endif
// Calculate the steps/s^2 acceleration rates, based on the mm/s^s // Calculate the steps/s^2 acceleration rates, based on the mm/s^s
void reset_acceleration_rates() void reset_acceleration_rates() {
{ for (int i = 0; i < NUM_AXIS; i++)
for(int8_t i=0; i < NUM_AXIS; i++)
{
axis_steps_per_sqr_second[i] = max_acceleration_units_per_sq_second[i] * axis_steps_per_unit[i]; axis_steps_per_sqr_second[i] = max_acceleration_units_per_sq_second[i] * axis_steps_per_unit[i];
} }
}

82
Marlin/planner.h
Unescape View File

@ -21,20 +21,16 @@
// This module is to be considered a sub-module of stepper.c. Please don't include // This module is to be considered a sub-module of stepper.c. Please don't include
// this file from any other module. // this file from any other module.
#ifndef planner_h #ifndef PLANNER_H
#define planner_h #define PLANNER_H
#include "Marlin.h" #include "Marlin.h"
#ifdef ENABLE_AUTO_BED_LEVELING
#include "vector_3.h"
#endif // ENABLE_AUTO_BED_LEVELING
// This struct is used when buffering the setup for each linear movement "nominal" values are as specified in // This struct is used when buffering the setup for each linear movement "nominal" values are as specified in
// the source g-code and may never actually be reached if acceleration management is active. // the source g-code and may never actually be reached if acceleration management is active.
typedef struct { typedef struct {
// Fields used by the bresenham algorithm for tracing the line // Fields used by the bresenham algorithm for tracing the line
long steps_x, steps_y, steps_z, steps_e; // Step count along each axis long steps[NUM_AXIS]; // Step count along each axis
unsigned long step_event_count; // The number of step events required to complete this block unsigned long step_event_count; // The number of step events required to complete this block
long accelerate_until; // The index of the step event on which to stop acceleration long accelerate_until; // The index of the step event on which to stop acceleration
long decelerate_after; // The index of the step event on which to start decelerating long decelerate_after; // The index of the step event on which to start decelerating
@ -71,43 +67,38 @@ typedef struct {
volatile char busy; volatile char busy;
} block_t; } block_t;
#ifdef ENABLE_AUTO_BED_LEVELING #define BLOCK_MOD(n) ((n)&(BLOCK_BUFFER_SIZE-1))
// this holds the required transform to compensate for bed level
extern matrix_3x3 plan_bed_level_matrix;
#endif // #ifdef ENABLE_AUTO_BED_LEVELING
// Initialize the motion plan subsystem // Initialize the motion plan subsystem
void plan_init(); void plan_init();
// Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in void check_axes_activity();
// millimaters. Feed rate specifies the speed of the motion.
// Get the number of buffered moves
extern volatile unsigned char block_buffer_head;
extern volatile unsigned char block_buffer_tail;
FORCE_INLINE uint8_t movesplanned() { return BLOCK_MOD(block_buffer_head - block_buffer_tail + BLOCK_BUFFER_SIZE); }
#if defined(ENABLE_AUTO_BED_LEVELING) || defined(MESH_BED_LEVELING) #if defined(ENABLE_AUTO_BED_LEVELING) || defined(MESH_BED_LEVELING)
void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate, const uint8_t &extruder);
#if defined(ENABLE_AUTO_BED_LEVELING) #if defined(ENABLE_AUTO_BED_LEVELING)
#ifndef DELTA #include "vector_3.h"
// this holds the required transform to compensate for bed level
extern matrix_3x3 plan_bed_level_matrix;
// Get the position applying the bed level matrix if enabled // Get the position applying the bed level matrix if enabled
vector_3 plan_get_position(); vector_3 plan_get_position();
#endif
#endif // ENABLE_AUTO_BED_LEVELING #endif // ENABLE_AUTO_BED_LEVELING
#else // Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in
void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t &extruder); // millimeters. Feed rate specifies the speed of the motion.
#endif // ENABLE_AUTO_BED_LEVELING || MESH_BED_LEVELING void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate, const uint8_t &extruder);
// Set position. Used for G92 instructions. // Set position. Used for G92 instructions.
#if defined(ENABLE_AUTO_BED_LEVELING) || defined(MESH_BED_LEVELING)
void plan_set_position(float x, float y, float z, const float &e); void plan_set_position(float x, float y, float z, const float &e);
#else #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 plan_set_position(const float &x, const float &y, const float &z, const float &e); void plan_set_position(const float &x, const float &y, const float &z, const float &e);
#endif // ENABLE_AUTO_BED_LEVELING || MESH_BED_LEVELING #endif // ENABLE_AUTO_BED_LEVELING || MESH_BED_LEVELING
void plan_set_e_position(const float &e); void plan_set_e_position(const float &e);
void check_axes_activity();
uint8_t movesplanned(); //return the nr of buffered moves
extern unsigned long minsegmenttime; extern unsigned long minsegmenttime;
extern float max_feedrate[NUM_AXIS]; // set the max speeds extern float max_feedrate[NUM_AXIS]; // set the max speeds
extern float axis_steps_per_unit[NUM_AXIS]; extern float axis_steps_per_unit[NUM_AXIS];
@ -129,38 +120,35 @@ extern unsigned long axis_steps_per_sqr_second[NUM_AXIS];
extern float autotemp_factor; extern float autotemp_factor;
#endif #endif
extern block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instructions
extern block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instfructions
extern volatile unsigned char block_buffer_head; // Index of the next block to be pushed extern volatile unsigned char block_buffer_head; // Index of the next block to be pushed
extern volatile unsigned char block_buffer_tail; extern volatile unsigned char block_buffer_tail;
// Called when the current block is no longer needed. Discards the block and makes the memory
// availible for new blocks. // Returns true if the buffer has a queued block, false otherwise
FORCE_INLINE void plan_discard_current_block() FORCE_INLINE bool blocks_queued() { return (block_buffer_head != block_buffer_tail); }
{
if (block_buffer_head != block_buffer_tail) { // Called when the current block is no longer needed. Discards
block_buffer_tail = (block_buffer_tail + 1) & (BLOCK_BUFFER_SIZE - 1); // the block and makes the memory available for new blocks.
} FORCE_INLINE void plan_discard_current_block() {
if (blocks_queued())
block_buffer_tail = BLOCK_MOD(block_buffer_tail + 1);
} }
// Gets the current block. Returns NULL if buffer empty // Gets the current block. Returns NULL if buffer empty
FORCE_INLINE block_t *plan_get_current_block() FORCE_INLINE block_t *plan_get_current_block() {
{ if (blocks_queued()) {
if (block_buffer_head == block_buffer_tail) {
return(NULL);
}
block_t *block = &block_buffer[block_buffer_tail]; block_t *block = &block_buffer[block_buffer_tail];
block->busy = true; block->busy = true;
return(block); return block;
}
else
return NULL;
} }
// Returns true if the buffer has a queued block, false otherwise
FORCE_INLINE bool blocks_queued() { return (block_buffer_head != block_buffer_tail); }
#ifdef PREVENT_DANGEROUS_EXTRUDE #ifdef PREVENT_DANGEROUS_EXTRUDE
void set_extrude_min_temp(float temp); void set_extrude_min_temp(float temp);
#endif #endif
void reset_acceleration_rates(); void reset_acceleration_rates();
#endif
#endif //PLANNER_H

39
Marlin/stepper.cpp
Unescape View File

@ -364,7 +364,7 @@ ISR(TIMER1_COMPA_vect) {
step_events_completed = 0; step_events_completed = 0;
#ifdef Z_LATE_ENABLE #ifdef Z_LATE_ENABLE
if (current_block->steps_z > 0) { if (current_block->steps[Z_AXIS] > 0) {
enable_z(); enable_z();
OCR1A = 2000; //1ms wait OCR1A = 2000; //1ms wait
return; return;
@ -405,7 +405,7 @@ ISR(TIMER1_COMPA_vect) {
#define UPDATE_ENDSTOP(axis,AXIS,minmax,MINMAX) \ #define UPDATE_ENDSTOP(axis,AXIS,minmax,MINMAX) \
bool axis ##_## minmax ##_endstop = (READ(AXIS ##_## MINMAX ##_PIN) != AXIS ##_## MINMAX ##_ENDSTOP_INVERTING); \ bool axis ##_## minmax ##_endstop = (READ(AXIS ##_## MINMAX ##_PIN) != AXIS ##_## MINMAX ##_ENDSTOP_INVERTING); \
if (axis ##_## minmax ##_endstop && old_## axis ##_## minmax ##_endstop && (current_block->steps_## axis > 0)) { \ if (axis ##_## minmax ##_endstop && old_## axis ##_## minmax ##_endstop && (current_block->steps[AXIS ##_AXIS] > 0)) { \
endstops_trigsteps[AXIS ##_AXIS] = count_position[AXIS ##_AXIS]; \ endstops_trigsteps[AXIS ##_AXIS] = count_position[AXIS ##_AXIS]; \
endstop_## axis ##_hit = true; \ endstop_## axis ##_hit = true; \
step_events_completed = current_block->step_event_count; \ step_events_completed = current_block->step_event_count; \
@ -414,13 +414,13 @@ ISR(TIMER1_COMPA_vect) {
// Check X and Y endstops // Check X and Y endstops
if (check_endstops) { if (check_endstops) {
#ifndef COREXY #ifdef COREXY
if (TEST(out_bits, X_AXIS)) // stepping along -X axis (regular cartesians bot)
#else
// Head direction in -X axis for CoreXY bots. // Head direction in -X axis for CoreXY bots.
// If DeltaX == -DeltaY, the movement is only in Y axis // If DeltaX == -DeltaY, the movement is only in Y axis
if (current_block->steps_x != current_block->steps_y || (TEST(out_bits, X_AXIS) == TEST(out_bits, Y_AXIS))) if (current_block->steps[A_AXIS] != current_block->steps[B_AXIS] || (TEST(out_bits, A_AXIS) == TEST(out_bits, B_AXIS)))
if (TEST(out_bits, X_HEAD)) if (TEST(out_bits, X_HEAD))
#else
if (TEST(out_bits, X_AXIS)) // stepping along -X axis (regular cartesians bot)
#endif #endif
{ // -direction { // -direction
#ifdef DUAL_X_CARRIAGE #ifdef DUAL_X_CARRIAGE
@ -444,13 +444,13 @@ ISR(TIMER1_COMPA_vect) {
#endif #endif
} }
} }
#ifndef COREXY #ifdef COREXY
if (TEST(out_bits, Y_AXIS)) // -direction
#else
// Head direction in -Y axis for CoreXY bots. // Head direction in -Y axis for CoreXY bots.
// If DeltaX == DeltaY, the movement is only in X axis // If DeltaX == DeltaY, the movement is only in X axis
if (current_block->steps_x != current_block->steps_y || (TEST(out_bits, X_AXIS) != TEST(out_bits, Y_AXIS))) if (current_block->steps[A_AXIS] != current_block->steps[B_AXIS] || (TEST(out_bits, A_AXIS) != TEST(out_bits, B_AXIS)))
if (TEST(out_bits, Y_HEAD)) if (TEST(out_bits, Y_HEAD))
#else
if (TEST(out_bits, Y_AXIS)) // -direction
#endif #endif
{ // -direction { // -direction
#if defined(Y_MIN_PIN) && Y_MIN_PIN >= 0 #if defined(Y_MIN_PIN) && Y_MIN_PIN >= 0
@ -509,7 +509,7 @@ ISR(TIMER1_COMPA_vect) {
#endif #endif
#ifdef ADVANCE #ifdef ADVANCE
counter_e += current_block->steps_e; counter_e += current_block->steps[E_AXIS];
if (counter_e > 0) { if (counter_e > 0) {
counter_e -= current_block->step_event_count; counter_e -= current_block->step_event_count;
e_steps[current_block->active_extruder] += TEST(out_bits, E_AXIS) ? -1 : 1; e_steps[current_block->active_extruder] += TEST(out_bits, E_AXIS) ? -1 : 1;
@ -523,15 +523,14 @@ ISR(TIMER1_COMPA_vect) {
* instead of doing each in turn. The extra tests add enough * instead of doing each in turn. The extra tests add enough
* lag to allow it work with without needing NOPs * lag to allow it work with without needing NOPs
*/ */
counter_x += current_block->steps_x; #define STEP_ADD(axis, AXIS) \
if (counter_x > 0) X_STEP_WRITE(HIGH); counter_## axis += current_block->steps[AXIS ##_AXIS]; \
counter_y += current_block->steps_y; if (counter_## axis > 0) { AXIS ##_STEP_WRITE(HIGH); }
if (counter_y > 0) Y_STEP_WRITE(HIGH); STEP_ADD(x,X);
counter_z += current_block->steps_z; STEP_ADD(y,Y);
if (counter_z > 0) Z_STEP_WRITE(HIGH); STEP_ADD(z,Z);
#ifndef ADVANCE #ifndef ADVANCE
counter_e += current_block->steps_e; STEP_ADD(e,E);
if (counter_e > 0) E_STEP_WRITE(HIGH);
#endif #endif
#define STEP_IF_COUNTER(axis, AXIS) \ #define STEP_IF_COUNTER(axis, AXIS) \
@ -551,7 +550,7 @@ ISR(TIMER1_COMPA_vect) {
#else // !CONFIG_STEPPERS_TOSHIBA #else // !CONFIG_STEPPERS_TOSHIBA
#define APPLY_MOVEMENT(axis, AXIS) \ #define APPLY_MOVEMENT(axis, AXIS) \
counter_## axis += current_block->steps_## axis; \ counter_## axis += current_block->steps[AXIS ##_AXIS]; \
if (counter_## axis > 0) { \ if (counter_## axis > 0) { \
AXIS ##_APPLY_STEP(!INVERT_## AXIS ##_STEP_PIN,0); \ AXIS ##_APPLY_STEP(!INVERT_## AXIS ##_STEP_PIN,0); \
counter_## axis -= current_block->step_event_count; \ counter_## axis -= current_block->step_event_count; \

Write Preview
Loading…
Cancel
Save