3DPrinting/Prusa-Firmware
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

Redefined the DDA step and accumulator values to unions to support

Browse source 
access to the low / high words of the 32bit values.
This is a prerequisity for an optimized 16bit only DDA
in case the number of step is lower than 32767.
This commit is contained in:
bubnikv 2018-01-14 17:01:04 +01:00
parent a1fd50ea9a
commit 30b06488ca
3 changed files with 119 additions and 98 deletions

108
Firmware/planner.cpp
View file

@ -227,8 +227,8 @@ void calculate_trapezoid_for_block(block_t *block, float entry_speed, float exit
// Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
// have to use intersection_distance() to calculate when to abort acceleration and start braking // have to use intersection_distance() to calculate when to abort acceleration and start braking
// in order to reach the final_rate exactly at the end of this block. // in order to reach the final_rate exactly at the end of this block.
if (accel_decel_steps < block->step_event_count) { if (accel_decel_steps < block->step_event_count.wide) {
plateau_steps = block->step_event_count - accel_decel_steps; plateau_steps = block->step_event_count.wide - accel_decel_steps;
} else { } else {
uint32_t acceleration_x4 = acceleration << 2; uint32_t acceleration_x4 = acceleration << 2;
// Avoid negative numbers // Avoid negative numbers
@ -240,26 +240,26 @@ void calculate_trapezoid_for_block(block_t *block, float entry_speed, float exit
accelerate_steps = (block->step_event_count >> 1) + (final_rate_sqr - initial_rate_sqr + acceleration_x4 - 1 + (block->step_event_count & 1) * acceleration_x2) / acceleration_x4; accelerate_steps = (block->step_event_count >> 1) + (final_rate_sqr - initial_rate_sqr + acceleration_x4 - 1 + (block->step_event_count & 1) * acceleration_x2) / acceleration_x4;
#else #else
accelerate_steps = final_rate_sqr - initial_rate_sqr + acceleration_x4 - 1; accelerate_steps = final_rate_sqr - initial_rate_sqr + acceleration_x4 - 1;
if (block->step_event_count & 1) if (block->step_event_count.wide & 1)
accelerate_steps += acceleration_x2; accelerate_steps += acceleration_x2;
accelerate_steps /= acceleration_x4; accelerate_steps /= acceleration_x4;
accelerate_steps += (block->step_event_count >> 1); accelerate_steps += (block->step_event_count.wide >> 1);
#endif #endif
if (accelerate_steps > block->step_event_count) if (accelerate_steps > block->step_event_count.wide)
accelerate_steps = block->step_event_count; accelerate_steps = block->step_event_count.wide;
} else { } else {
#if 0 #if 0
decelerate_steps = (block->step_event_count >> 1) + (initial_rate_sqr - final_rate_sqr + (block->step_event_count & 1) * acceleration_x2) / acceleration_x4; decelerate_steps = (block->step_event_count >> 1) + (initial_rate_sqr - final_rate_sqr + (block->step_event_count & 1) * acceleration_x2) / acceleration_x4;
#else #else
decelerate_steps = initial_rate_sqr - final_rate_sqr; decelerate_steps = initial_rate_sqr - final_rate_sqr;
if (block->step_event_count & 1) if (block->step_event_count.wide & 1)
decelerate_steps += acceleration_x2; decelerate_steps += acceleration_x2;
decelerate_steps /= acceleration_x4; decelerate_steps /= acceleration_x4;
decelerate_steps += (block->step_event_count >> 1); decelerate_steps += (block->step_event_count.wide >> 1);
#endif #endif
if (decelerate_steps > block->step_event_count) if (decelerate_steps > block->step_event_count.wide)
decelerate_steps = block->step_event_count; decelerate_steps = block->step_event_count.wide;
accelerate_steps = block->step_event_count - decelerate_steps; accelerate_steps = block->step_event_count.wide - decelerate_steps;
} }
} }
@ -449,10 +449,10 @@ void getHighESpeed()
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) || if((block_buffer[block_index].steps_x.wide != 0) ||
(block_buffer[block_index].steps_y != 0) || (block_buffer[block_index].steps_y.wide != 0) ||
(block_buffer[block_index].steps_z != 0)) { (block_buffer[block_index].steps_z.wide != 0)) {
float se=(float(block_buffer[block_index].steps_e)/float(block_buffer[block_index].step_event_count))*block_buffer[block_index].nominal_speed; float se=(float(block_buffer[block_index].steps_e.wide)/float(block_buffer[block_index].step_event_count.wide))*block_buffer[block_index].nominal_speed;
//se; mm/sec; //se; mm/sec;
if(se>high) if(se>high)
{ {
@ -493,10 +493,10 @@ void check_axes_activity()
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++; if(block->steps_x.wide != 0) x_active++;
if(block->steps_y != 0) y_active++; if(block->steps_y.wide != 0) y_active++;
if(block->steps_z != 0) z_active++; if(block->steps_z.wide != 0) z_active++;
if(block->steps_e != 0) e_active++; if(block->steps_e.wide != 0) e_active++;
block_index = (block_index+1) & (BLOCK_BUFFER_SIZE - 1); block_index = (block_index+1) & (BLOCK_BUFFER_SIZE - 1);
} }
} }
@ -769,26 +769,24 @@ void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate
// Number of steps for each axis // Number of steps for each axis
#ifndef COREXY #ifndef COREXY
// default non-h-bot planning // default non-h-bot planning
block->steps_x = labs(target[X_AXIS]-position[X_AXIS]); block->steps_x.wide = labs(target[X_AXIS]-position[X_AXIS]);
block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]); block->steps_y.wide = labs(target[Y_AXIS]-position[Y_AXIS]);
#else #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_x.wide = labs((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]));
block->steps_y = labs((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS])); block->steps_y.wide = labs((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]));
#endif #endif
block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]); block->steps_z.wide = labs(target[Z_AXIS]-position[Z_AXIS]);
block->steps_e = labs(target[E_AXIS]-position[E_AXIS]); block->steps_e.wide = labs(target[E_AXIS]-position[E_AXIS]);
if (volumetric_multiplier[active_extruder] != 1.f) if (volumetric_multiplier[active_extruder] != 1.f)
block->steps_e *= volumetric_multiplier[active_extruder]; block->steps_e.wide *= volumetric_multiplier[active_extruder];
if (extrudemultiply != 100) { if (extrudemultiply != 100)
block->steps_e *= extrudemultiply; block->steps_e.wide *= extrudemultiply * 0.01;
block->steps_e /= 100; block->step_event_count.wide = max(block->steps_x.wide, max(block->steps_y.wide, max(block->steps_z.wide, block->steps_e.wide)));
}
block->step_event_count = max(block->steps_x, max(block->steps_y, max(block->steps_z, block->steps_e)));
// 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.wide <= dropsegments)
{ {
#ifdef PLANNER_DIAGNOSTICS #ifdef PLANNER_DIAGNOSTICS
planner_update_queue_min_counter(); planner_update_queue_min_counter();
@ -832,21 +830,21 @@ block->steps_y = labs((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-positi
//enable active axes //enable active axes
#ifdef COREXY #ifdef COREXY
if((block->steps_x != 0) || (block->steps_y != 0)) if((block->steps_x.wide != 0) || (block->steps_y.wide != 0))
{ {
enable_x(); enable_x();
enable_y(); enable_y();
} }
#else #else
if(block->steps_x != 0) enable_x(); if(block->steps_x.wide != 0) enable_x();
if(block->steps_y != 0) enable_y(); if(block->steps_y.wide != 0) enable_y();
#endif #endif
#ifndef Z_LATE_ENABLE #ifndef Z_LATE_ENABLE
if(block->steps_z != 0) enable_z(); if(block->steps_z.wide != 0) enable_z();
#endif #endif
// Enable extruder(s) // Enable extruder(s)
if(block->steps_e != 0) if(block->steps_e.wide != 0)
{ {
if (DISABLE_INACTIVE_EXTRUDER) //enable only selected extruder if (DISABLE_INACTIVE_EXTRUDER) //enable only selected extruder
{ {
@ -888,7 +886,7 @@ block->steps_y = labs((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-positi
} }
} }
if (block->steps_e == 0) if (block->steps_e.wide == 0)
{ {
if(feed_rate<mintravelfeedrate) feed_rate=mintravelfeedrate; if(feed_rate<mintravelfeedrate) feed_rate=mintravelfeedrate;
} }
@ -917,7 +915,7 @@ Having the real displacement of the head, we can calculate the total movement le
#endif #endif
delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/axis_steps_per_unit[Z_AXIS]; delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/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] = ((target[E_AXIS]-position[E_AXIS])/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.wide <=dropsegments && block->steps_y.wide <=dropsegments && block->steps_z.wide <=dropsegments )
{ {
block->millimeters = fabs(delta_mm[E_AXIS]); block->millimeters = fabs(delta_mm[E_AXIS]);
} }
@ -950,7 +948,7 @@ Having the real displacement of the head, we can calculate the total movement le
#endif // SLOWDOWN #endif // SLOWDOWN
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.wide * 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
@ -1027,8 +1025,8 @@ Having the real displacement of the head, we can calculate the total movement le
// Compute and limit the acceleration rate for the trapezoid generator. // Compute and limit the acceleration rate for the trapezoid generator.
// block->step_event_count ... event count of the fastest axis // block->step_event_count ... event count of the fastest axis
// block->millimeters ... Euclidian length of the XYZ movement or the E length, if no XYZ movement. // block->millimeters ... Euclidian length of the XYZ movement or the E length, if no XYZ movement.
float steps_per_mm = block->step_event_count/block->millimeters; float steps_per_mm = block->step_event_count.wide/block->millimeters;
if(block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0) if(block->steps_x.wide == 0 && block->steps_y.wide == 0 && block->steps_z.wide == 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
} }
@ -1038,29 +1036,29 @@ Having the real displacement of the head, we can calculate the total movement le
#ifdef TMC2130 #ifdef TMC2130
if (tmc2130_mode == TMC2130_MODE_SILENT) if (tmc2130_mode == TMC2130_MODE_SILENT)
{ {
if(((float)block->acceleration_st * (float)block->steps_x / (float)block->step_event_count) > SILENT_MAX_ACCEL_X_ST) if(((float)block->acceleration_st * (float)block->steps_x.wide / (float)block->step_event_count.wide) > SILENT_MAX_ACCEL_X_ST)
block->acceleration_st = SILENT_MAX_ACCEL_X_ST; block->acceleration_st = SILENT_MAX_ACCEL_X_ST;
if(((float)block->acceleration_st * (float)block->steps_y / (float)block->step_event_count) > SILENT_MAX_ACCEL_Y_ST) if(((float)block->acceleration_st * (float)block->steps_y.wide / (float)block->step_event_count.wide) > SILENT_MAX_ACCEL_Y_ST)
block->acceleration_st = SILENT_MAX_ACCEL_Y_ST; block->acceleration_st = SILENT_MAX_ACCEL_Y_ST;
} }
if(((float)block->acceleration_st * (float)block->steps_x / (float)block->step_event_count) > axis_steps_per_sqr_second[X_AXIS]) if(((float)block->acceleration_st * (float)block->steps_x.wide / (float)block->step_event_count.wide) > axis_steps_per_sqr_second[X_AXIS])
block->acceleration_st = axis_steps_per_sqr_second[X_AXIS]; block->acceleration_st = 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]) if(((float)block->acceleration_st * (float)block->steps_y.wide / (float)block->step_event_count.wide) > axis_steps_per_sqr_second[Y_AXIS])
block->acceleration_st = axis_steps_per_sqr_second[Y_AXIS]; block->acceleration_st = axis_steps_per_sqr_second[Y_AXIS];
if(((float)block->acceleration_st * (float)block->steps_e / (float)block->step_event_count) > axis_steps_per_sqr_second[E_AXIS]) if(((float)block->acceleration_st * (float)block->steps_e.wide / (float)block->step_event_count.wide) > axis_steps_per_sqr_second[E_AXIS])
block->acceleration_st = axis_steps_per_sqr_second[E_AXIS]; block->acceleration_st = axis_steps_per_sqr_second[E_AXIS];
if(((float)block->acceleration_st * (float)block->steps_z / (float)block->step_event_count ) > axis_steps_per_sqr_second[Z_AXIS]) if(((float)block->acceleration_st * (float)block->steps_z.wide / (float)block->step_event_count.wide ) > axis_steps_per_sqr_second[Z_AXIS])
block->acceleration_st = axis_steps_per_sqr_second[Z_AXIS]; block->acceleration_st = axis_steps_per_sqr_second[Z_AXIS];
#else //TMC2130 #else //TMC2130
// Limit acceleration per axis // Limit acceleration per axis
//FIXME Vojtech: One shall rather limit a projection of the acceleration vector instead of using the limit. //FIXME Vojtech: One shall rather limit a projection of the acceleration vector instead of using the limit.
if(((float)block->acceleration_st * (float)block->steps_x / (float)block->step_event_count) > axis_steps_per_sqr_second[X_AXIS]) if(((float)block->acceleration_st * (float)block->steps_x.wide / (float)block->step_event_count.wide) > axis_steps_per_sqr_second[X_AXIS])
block->acceleration_st = axis_steps_per_sqr_second[X_AXIS]; block->acceleration_st = 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]) if(((float)block->acceleration_st * (float)block->steps_y.wide / (float)block->step_event_count.wide) > axis_steps_per_sqr_second[Y_AXIS])
block->acceleration_st = axis_steps_per_sqr_second[Y_AXIS]; block->acceleration_st = axis_steps_per_sqr_second[Y_AXIS];
if(((float)block->acceleration_st * (float)block->steps_e / (float)block->step_event_count) > axis_steps_per_sqr_second[E_AXIS]) if(((float)block->acceleration_st * (float)block->steps_e.wide / (float)block->step_event_count.wide) > axis_steps_per_sqr_second[E_AXIS])
block->acceleration_st = axis_steps_per_sqr_second[E_AXIS]; block->acceleration_st = axis_steps_per_sqr_second[E_AXIS];
if(((float)block->acceleration_st * (float)block->steps_z / (float)block->step_event_count ) > axis_steps_per_sqr_second[Z_AXIS]) if(((float)block->acceleration_st * (float)block->steps_z.wide / (float)block->step_event_count.wide ) > axis_steps_per_sqr_second[Z_AXIS])
block->acceleration_st = axis_steps_per_sqr_second[Z_AXIS]; block->acceleration_st = axis_steps_per_sqr_second[Z_AXIS];
#endif //TMC2130 #endif //TMC2130
} }
@ -1218,10 +1216,10 @@ Having the real displacement of the head, we can calculate the total movement le
// The math is good, but we must avoid retract moves with advance! // The math is good, but we must avoid retract moves with advance!
// de_float > 0.0 : Extruder is running forward (e.g., for "Wipe while retracting" (Slic3r) or "Combing" (Cura) moves) // de_float > 0.0 : Extruder is running forward (e.g., for "Wipe while retracting" (Slic3r) or "Combing" (Cura) moves)
// //
block->use_advance_lead = block->steps_e block->use_advance_lead = block->steps_e.wide
&& (block->steps_x || block->steps_y) && (block->steps_x.wide || block->steps_y.wide)
&& extruder_advance_k && extruder_advance_k
&& (uint32_t)block->steps_e != block->step_event_count && (uint32_t)block->steps_e.wide != block->step_event_count.wide
&& de_float > 0.0; && de_float > 0.0;
if (block->use_advance_lead) if (block->use_advance_lead)
block->abs_adv_steps_multiplier8 = lround( block->abs_adv_steps_multiplier8 = lround(

26
Firmware/planner.h
View file

@ -40,6 +40,28 @@ enum BlockFlag {
// If set, the machine will start from a halt at the start of this block, // If set, the machine will start from a halt at the start of this block,
// respecting the maximum allowed jerk. // respecting the maximum allowed jerk.
BLOCK_FLAG_START_FROM_FULL_HALT = 4, BLOCK_FLAG_START_FROM_FULL_HALT = 4,
// If set, the stepper interrupt expects, that the number of steps to tick will be lower
// than 32767, therefore the DDA algorithm may run with 16bit resolution only.
// In addition, the stepper routine will not do any end stop checking for higher performance.
BLOCK_FLAG_DDA_LOWRES = 8,
};
union dda_isteps_t
{
int32_t wide;
struct {
uint16_t lo;
int16_t hi;
};
};
union dda_usteps_t
{
uint32_t wide;
struct {
uint16_t lo;
uint16_t hi;
};
}; };
// 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
@ -47,8 +69,8 @@ enum BlockFlag {
typedef struct { typedef struct {
// Fields used by the bresenham algorithm for tracing the line // Fields used by the bresenham algorithm for tracing the line
// steps_x.y,z, step_event_count, acceleration_rate, direction_bits and active_extruder are set by plan_buffer_line(). // steps_x.y,z, step_event_count, acceleration_rate, direction_bits and active_extruder are set by plan_buffer_line().
long steps_x, steps_y, steps_z, steps_e; // Step count along each axis dda_isteps_t steps_x, steps_y, steps_z, steps_e; // Step count along each axis
unsigned long step_event_count; // The number of step events required to complete this block dda_usteps_t step_event_count; // The number of step events required to complete this block
long acceleration_rate; // The acceleration rate used for acceleration calculation long acceleration_rate; // The acceleration rate used for acceleration calculation
unsigned char direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) unsigned char direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h)
unsigned char active_extruder; // Selects the active extruder unsigned char active_extruder; // Selects the active extruder

83
Firmware/stepper.cpp
View file

@ -62,11 +62,12 @@ bool z_max_endstop = false;
// Variables used by The Stepper Driver Interrupt // Variables used by The Stepper Driver Interrupt
static unsigned char out_bits; // The next stepping-bits to be output static unsigned char out_bits; // The next stepping-bits to be output
static int32_t counter_x, // Counter variables for the bresenham line tracer static dda_isteps_t
counter_x, // Counter variables for the bresenham line tracer
counter_y, counter_y,
counter_z, counter_z,
counter_e; counter_e;
volatile uint32_t step_events_completed; // The number of step events executed in the current block volatile dda_usteps_t step_events_completed; // The number of step events executed in the current block
static int32_t acceleration_time, deceleration_time; static int32_t acceleration_time, deceleration_time;
//static unsigned long accelerate_until, decelerate_after, acceleration_rate, initial_rate, final_rate, nominal_rate; //static unsigned long accelerate_until, decelerate_after, acceleration_rate, initial_rate, final_rate, nominal_rate;
static uint16_t acc_step_rate; // needed for deccelaration start point static uint16_t acc_step_rate; // needed for deccelaration start point
@ -404,14 +405,14 @@ void isr() {
// The busy flag is set by the plan_get_current_block() call. // The busy flag is set by the plan_get_current_block() call.
// current_block->busy = true; // current_block->busy = true;
trapezoid_generator_reset(); trapezoid_generator_reset();
counter_x = -(current_block->step_event_count >> 1); counter_x.wide = -(current_block->step_event_count.wide >> 1);
counter_y = counter_x; counter_y.wide = counter_x.wide;
counter_z = counter_x; counter_z.wide = counter_x.wide;
counter_e = counter_x; counter_e.wide = counter_x.wide;
step_events_completed = 0; step_events_completed.wide = 0;
#ifdef Z_LATE_ENABLE #ifdef Z_LATE_ENABLE
if(current_block->steps_z > 0) { if(current_block->steps_z.wide > 0) {
enable_z(); enable_z();
_NEXT_ISR(2000); //1ms wait _NEXT_ISR(2000); //1ms wait
return; return;
@ -476,10 +477,10 @@ void isr() {
// Normal homing // Normal homing
x_min_endstop = (READ(X_MIN_PIN) != X_MIN_ENDSTOP_INVERTING); x_min_endstop = (READ(X_MIN_PIN) != X_MIN_ENDSTOP_INVERTING);
#endif #endif
if(x_min_endstop && old_x_min_endstop && (current_block->steps_x > 0)) { if(x_min_endstop && old_x_min_endstop && (current_block->steps_x.wide > 0)) {
endstops_trigsteps[X_AXIS] = count_position[X_AXIS]; endstops_trigsteps[X_AXIS] = count_position[X_AXIS];
endstop_x_hit=true; endstop_x_hit=true;
step_events_completed = current_block->step_event_count; step_events_completed.wide = current_block->step_event_count.wide;
} }
old_x_min_endstop = x_min_endstop; old_x_min_endstop = x_min_endstop;
#endif #endif
@ -499,10 +500,10 @@ void isr() {
// Normal homing // Normal homing
x_max_endstop = (READ(X_MAX_PIN) != X_MAX_ENDSTOP_INVERTING); x_max_endstop = (READ(X_MAX_PIN) != X_MAX_ENDSTOP_INVERTING);
#endif #endif
if(x_max_endstop && old_x_max_endstop && (current_block->steps_x > 0)){ if(x_max_endstop && old_x_max_endstop && (current_block->steps_x.wide > 0)){
endstops_trigsteps[X_AXIS] = count_position[X_AXIS]; endstops_trigsteps[X_AXIS] = count_position[X_AXIS];
endstop_x_hit=true; endstop_x_hit=true;
step_events_completed = current_block->step_event_count; step_events_completed.wide = current_block->step_event_count.wide;
} }
old_x_max_endstop = x_max_endstop; old_x_max_endstop = x_max_endstop;
#endif #endif
@ -527,10 +528,10 @@ void isr() {
// Normal homing // Normal homing
y_min_endstop = (READ(Y_MIN_PIN) != Y_MIN_ENDSTOP_INVERTING); y_min_endstop = (READ(Y_MIN_PIN) != Y_MIN_ENDSTOP_INVERTING);
#endif #endif
if(y_min_endstop && old_y_min_endstop && (current_block->steps_y > 0)) { if(y_min_endstop && old_y_min_endstop && (current_block->steps_y.wide > 0)) {
endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS]; endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS];
endstop_y_hit=true; endstop_y_hit=true;
step_events_completed = current_block->step_event_count; step_events_completed.wide = current_block->step_event_count.wide;
} }
old_y_min_endstop = y_min_endstop; old_y_min_endstop = y_min_endstop;
#endif #endif
@ -548,10 +549,10 @@ void isr() {
// Normal homing // Normal homing
y_max_endstop = (READ(Y_MAX_PIN) != Y_MAX_ENDSTOP_INVERTING); y_max_endstop = (READ(Y_MAX_PIN) != Y_MAX_ENDSTOP_INVERTING);
#endif #endif
if(y_max_endstop && old_y_max_endstop && (current_block->steps_y > 0)){ if(y_max_endstop && old_y_max_endstop && (current_block->steps_y.wide > 0)){
endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS]; endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS];
endstop_y_hit=true; endstop_y_hit=true;
step_events_completed = current_block->step_event_count; step_events_completed.wide = current_block->step_event_count.wide;
} }
old_y_max_endstop = y_max_endstop; old_y_max_endstop = y_max_endstop;
#endif #endif
@ -575,10 +576,10 @@ void isr() {
#else #else
z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING); z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING);
#endif //TMC2130_SG_HOMING #endif //TMC2130_SG_HOMING
if(z_min_endstop && old_z_min_endstop && (current_block->steps_z > 0)) { if(z_min_endstop && old_z_min_endstop && (current_block->steps_z.wide > 0)) {
endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS]; endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
endstop_z_hit=true; endstop_z_hit=true;
step_events_completed = current_block->step_event_count; step_events_completed.wide = current_block->step_event_count.wide;
} }
old_z_min_endstop = z_min_endstop; old_z_min_endstop = z_min_endstop;
#endif #endif
@ -601,10 +602,10 @@ void isr() {
#else #else
z_max_endstop = (READ(Z_MAX_PIN) != Z_MAX_ENDSTOP_INVERTING); z_max_endstop = (READ(Z_MAX_PIN) != Z_MAX_ENDSTOP_INVERTING);
#endif //TMC2130_SG_HOMING #endif //TMC2130_SG_HOMING
if(z_max_endstop && old_z_max_endstop && (current_block->steps_z > 0)) { if(z_max_endstop && old_z_max_endstop && (current_block->steps_z.wide > 0)) {
endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS]; endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
endstop_z_hit=true; endstop_z_hit=true;
step_events_completed = current_block->step_event_count; step_events_completed.wide = current_block->step_event_count.wide;
} }
old_z_max_endstop = z_max_endstop; old_z_max_endstop = z_max_endstop;
#endif #endif
@ -625,7 +626,7 @@ void isr() {
if(z_min_endstop && old_z_min_endstop) { if(z_min_endstop && old_z_min_endstop) {
endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS]; endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
endstop_z_hit=true; endstop_z_hit=true;
step_events_completed = current_block->step_event_count; step_events_completed.wide = current_block->step_event_count.wide;
} }
old_z_min_endstop = z_min_endstop; old_z_min_endstop = z_min_endstop;
} }
@ -657,22 +658,22 @@ void isr() {
#endif //RP - returned, because missing characters #endif //RP - returned, because missing characters
#ifdef LIN_ADVANCE #ifdef LIN_ADVANCE
counter_e += current_block->steps_e; counter_e.wide += current_block->steps_e.wide;
if (counter_e > 0) { if (counter_e.wide > 0) {
counter_e -= current_block->step_event_count; counter_e.wide -= current_block->step_event_count.wide;
count_position[E_AXIS] += count_direction[E_AXIS]; count_position[E_AXIS] += count_direction[E_AXIS];
((out_bits&(1<<E_AXIS))!=0) ? --e_steps : ++e_steps; ((out_bits&(1<<E_AXIS))!=0) ? --e_steps : ++e_steps;
} }
#endif #endif
counter_x += current_block->steps_x; counter_x.wide += current_block->steps_x.wide;
if (counter_x > 0) { if (counter_x.wide > 0) {
WRITE_NC(X_STEP_PIN, !INVERT_X_STEP_PIN); WRITE_NC(X_STEP_PIN, !INVERT_X_STEP_PIN);
LastStepMask |= X_AXIS_MASK; LastStepMask |= X_AXIS_MASK;
#ifdef DEBUG_XSTEP_DUP_PIN #ifdef DEBUG_XSTEP_DUP_PIN
WRITE_NC(DEBUG_XSTEP_DUP_PIN,!INVERT_X_STEP_PIN); WRITE_NC(DEBUG_XSTEP_DUP_PIN,!INVERT_X_STEP_PIN);
#endif //DEBUG_XSTEP_DUP_PIN #endif //DEBUG_XSTEP_DUP_PIN
counter_x -= current_block->step_event_count; counter_x.wide -= current_block->step_event_count.wide;
count_position[X_AXIS]+=count_direction[X_AXIS]; count_position[X_AXIS]+=count_direction[X_AXIS];
WRITE_NC(X_STEP_PIN, INVERT_X_STEP_PIN); WRITE_NC(X_STEP_PIN, INVERT_X_STEP_PIN);
#ifdef DEBUG_XSTEP_DUP_PIN #ifdef DEBUG_XSTEP_DUP_PIN
@ -680,8 +681,8 @@ void isr() {
#endif //DEBUG_XSTEP_DUP_PIN #endif //DEBUG_XSTEP_DUP_PIN
} }
counter_y += current_block->steps_y; counter_y.wide += current_block->steps_y.wide;
if (counter_y > 0) { if (counter_y.wide > 0) {
WRITE_NC(Y_STEP_PIN, !INVERT_Y_STEP_PIN); WRITE_NC(Y_STEP_PIN, !INVERT_Y_STEP_PIN);
LastStepMask |= Y_AXIS_MASK; LastStepMask |= Y_AXIS_MASK;
#ifdef DEBUG_YSTEP_DUP_PIN #ifdef DEBUG_YSTEP_DUP_PIN
@ -692,7 +693,7 @@ void isr() {
WRITE_NC(Y2_STEP_PIN, !INVERT_Y_STEP_PIN); WRITE_NC(Y2_STEP_PIN, !INVERT_Y_STEP_PIN);
#endif #endif
counter_y -= current_block->step_event_count; counter_y.wide -= current_block->step_event_count.wide;
count_position[Y_AXIS]+=count_direction[Y_AXIS]; count_position[Y_AXIS]+=count_direction[Y_AXIS];
WRITE_NC(Y_STEP_PIN, INVERT_Y_STEP_PIN); WRITE_NC(Y_STEP_PIN, INVERT_Y_STEP_PIN);
#ifdef DEBUG_YSTEP_DUP_PIN #ifdef DEBUG_YSTEP_DUP_PIN
@ -704,15 +705,15 @@ void isr() {
#endif #endif
} }
counter_z += current_block->steps_z; counter_z.wide += current_block->steps_z.wide;
if (counter_z > 0) { if (counter_z.wide > 0) {
WRITE_NC(Z_STEP_PIN, !INVERT_Z_STEP_PIN); WRITE_NC(Z_STEP_PIN, !INVERT_Z_STEP_PIN);
LastStepMask |= Z_AXIS_MASK; LastStepMask |= Z_AXIS_MASK;
#ifdef Z_DUAL_STEPPER_DRIVERS #ifdef Z_DUAL_STEPPER_DRIVERS
WRITE_NC(Z2_STEP_PIN, !INVERT_Z_STEP_PIN); WRITE_NC(Z2_STEP_PIN, !INVERT_Z_STEP_PIN);
#endif #endif
counter_z -= current_block->step_event_count; counter_z.wide -= current_block->step_event_count.wide;
count_position[Z_AXIS]+=count_direction[Z_AXIS]; count_position[Z_AXIS]+=count_direction[Z_AXIS];
WRITE_NC(Z_STEP_PIN, INVERT_Z_STEP_PIN); WRITE_NC(Z_STEP_PIN, INVERT_Z_STEP_PIN);
@ -722,10 +723,10 @@ void isr() {
} }
#ifndef LIN_ADVANCE #ifndef LIN_ADVANCE
counter_e += current_block->steps_e; counter_e.wide += current_block->steps_e.wide;
if (counter_e > 0) { if (counter_e.wide > 0) {
WRITE(E0_STEP_PIN, !INVERT_E_STEP_PIN); WRITE(E0_STEP_PIN, !INVERT_E_STEP_PIN);
counter_e -= current_block->step_event_count; counter_e.wide -= current_block->step_event_count.wide;
count_position[E_AXIS]+=count_direction[E_AXIS]; count_position[E_AXIS]+=count_direction[E_AXIS];
WRITE(E0_STEP_PIN, INVERT_E_STEP_PIN); WRITE(E0_STEP_PIN, INVERT_E_STEP_PIN);
#ifdef PAT9125 #ifdef PAT9125
@ -734,8 +735,8 @@ void isr() {
} }
#endif #endif
step_events_completed += 1; ++ step_events_completed.wide;
if(step_events_completed >= current_block->step_event_count) break; if(step_events_completed.wide >= current_block->step_event_count.wide) break;
} }
#ifdef LIN_ADVANCE #ifdef LIN_ADVANCE
if (current_block->use_advance_lead) { if (current_block->use_advance_lead) {
@ -750,7 +751,7 @@ void isr() {
// Calculare new timer value // Calculare new timer value
unsigned short timer; unsigned short timer;
uint16_t step_rate; uint16_t step_rate;
if (step_events_completed <= (unsigned long int)current_block->accelerate_until) { if (step_events_completed.wide <= (unsigned long int)current_block->accelerate_until) {
// v = t * a -> acc_step_rate = acceleration_time * current_block->acceleration_rate // v = t * a -> acc_step_rate = acceleration_time * current_block->acceleration_rate
MultiU24X24toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate); MultiU24X24toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate);
acc_step_rate += current_block->initial_rate; acc_step_rate += current_block->initial_rate;
@ -771,7 +772,7 @@ void isr() {
eISR_Rate = ADV_RATE(timer, step_loops); eISR_Rate = ADV_RATE(timer, step_loops);
#endif #endif
} }
else if (step_events_completed > (unsigned long int)current_block->decelerate_after) { else if (step_events_completed.wide > (unsigned long int)current_block->decelerate_after) {
MultiU24X24toH16(step_rate, deceleration_time, current_block->acceleration_rate); MultiU24X24toH16(step_rate, deceleration_time, current_block->acceleration_rate);
if(step_rate > acc_step_rate) { // Check step_rate stays positive if(step_rate > acc_step_rate) { // Check step_rate stays positive
@ -811,7 +812,7 @@ void isr() {
} }
// If current block is finished, reset pointer // If current block is finished, reset pointer
if (step_events_completed >= current_block->step_event_count) { if (step_events_completed.wide >= current_block->step_event_count.wide) {
#ifdef PAT9125 #ifdef PAT9125
fsensor_st_block_chunk(current_block, fsensor_counter); fsensor_st_block_chunk(current_block, fsensor_counter);