3DPrinting/Prusa-Firmware
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases 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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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
// 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.
if (accel_decel_steps < block->step_event_count) {
plateau_steps = block->step_event_count - accel_decel_steps;
if (accel_decel_steps < block->step_event_count.wide) {
plateau_steps = block->step_event_count.wide - accel_decel_steps;
} else {
uint32_t acceleration_x4 = acceleration << 2;
// 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;
#else
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_x4;
accelerate_steps += (block->step_event_count >> 1);
accelerate_steps += (block->step_event_count.wide >> 1);
#endif
if (accelerate_steps > block->step_event_count)
accelerate_steps = block->step_event_count;
if (accelerate_steps > block->step_event_count.wide)
accelerate_steps = block->step_event_count.wide;
} else {
#if 0
decelerate_steps = (block->step_event_count >> 1) + (initial_rate_sqr - final_rate_sqr + (block->step_event_count & 1) * acceleration_x2) / acceleration_x4;
#else
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_x4;
decelerate_steps += (block->step_event_count >> 1);
decelerate_steps += (block->step_event_count.wide >> 1);
#endif
if (decelerate_steps > block->step_event_count)
decelerate_steps = block->step_event_count;
accelerate_steps = block->step_event_count - decelerate_steps;
if (decelerate_steps > block->step_event_count.wide)
decelerate_steps = block->step_event_count.wide;
accelerate_steps = block->step_event_count.wide - decelerate_steps;
}
}
@ -449,10 +449,10 @@ void getHighESpeed()
uint8_t block_index = block_buffer_tail;
while(block_index != block_buffer_head) {
if((block_buffer[block_index].steps_x != 0) ||
(block_buffer[block_index].steps_y != 0) ||
(block_buffer[block_index].steps_z != 0)) {
float se=(float(block_buffer[block_index].steps_e)/float(block_buffer[block_index].step_event_count))*block_buffer[block_index].nominal_speed;
if((block_buffer[block_index].steps_x.wide != 0) ||
(block_buffer[block_index].steps_y.wide != 0) ||
(block_buffer[block_index].steps_z.wide != 0)) {
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;
if(se>high)
{
@ -493,10 +493,10 @@ void check_axes_activity()
while(block_index != block_buffer_head)
{
block = &block_buffer[block_index];
if(block->steps_x != 0) x_active++;
if(block->steps_y != 0) y_active++;
if(block->steps_z != 0) z_active++;
if(block->steps_e != 0) e_active++;
if(block->steps_x.wide != 0) x_active++;
if(block->steps_y.wide != 0) y_active++;
if(block->steps_z.wide != 0) z_active++;
if(block->steps_e.wide != 0) e_active++;
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
#ifndef 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]);
block->steps_x.wide = labs(target[X_AXIS]-position[X_AXIS]);
block->steps_y.wide = labs(target[Y_AXIS]-position[Y_AXIS]);
#else
// corexy planning
// 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_y = 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.wide = labs((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]));
#endif
block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]);
block->steps_e = labs(target[E_AXIS]-position[E_AXIS]);
block->steps_z.wide = labs(target[Z_AXIS]-position[Z_AXIS]);
block->steps_e.wide = labs(target[E_AXIS]-position[E_AXIS]);
if (volumetric_multiplier[active_extruder] != 1.f)
block->steps_e *= volumetric_multiplier[active_extruder];
if (extrudemultiply != 100) {
block->steps_e *= extrudemultiply;
block->steps_e /= 100;
}
block->step_event_count = max(block->steps_x, max(block->steps_y, max(block->steps_z, block->steps_e)));
block->steps_e.wide *= volumetric_multiplier[active_extruder];
if (extrudemultiply != 100)
block->steps_e.wide *= extrudemultiply * 0.01;
block->step_event_count.wide = max(block->steps_x.wide, max(block->steps_y.wide, max(block->steps_z.wide, block->steps_e.wide)));
// Bail if this is a zero-length block
if (block->step_event_count <= dropsegments)
if (block->step_event_count.wide <= dropsegments)
{
#ifdef PLANNER_DIAGNOSTICS
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
#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_y();
}
#else
if(block->steps_x != 0) enable_x();
if(block->steps_y != 0) enable_y();
if(block->steps_x.wide != 0) enable_x();
if(block->steps_y.wide != 0) enable_y();
#endif
#ifndef Z_LATE_ENABLE
if(block->steps_z != 0) enable_z();
if(block->steps_z.wide != 0) enable_z();
#endif
// Enable extruder(s)
if(block->steps_e != 0)
if(block->steps_e.wide != 0)
{
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;
}
@ -917,7 +915,7 @@ Having the real displacement of the head, we can calculate the total movement le
#endif
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;
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]);
}
@ -950,7 +948,7 @@ Having the real displacement of the head, we can calculate the total movement le
#endif // SLOWDOWN
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
//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.
// 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.
float steps_per_mm = block->step_event_count/block->millimeters;
if(block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0)
float steps_per_mm = block->step_event_count.wide/block->millimeters;
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
}
@ -1038,29 +1036,29 @@ Having the real displacement of the head, we can calculate the total movement le
#ifdef TMC2130
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;
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;
}
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];
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];
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];
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];
#else //TMC2130
// Limit acceleration per axis
//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];
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];
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];
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];
#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!
// 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->steps_x || block->steps_y)
block->use_advance_lead = block->steps_e.wide
&& (block->steps_x.wide || block->steps_y.wide)
&& 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;
if (block->use_advance_lead)
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,
// respecting the maximum allowed jerk.
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
@ -47,8 +69,8 @@ enum BlockFlag {
typedef struct {
// 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().
long 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_isteps_t steps_x, steps_y, steps_z, steps_e; // Step count along each axis
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
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

83
Firmware/stepper.cpp
View File

@ -62,11 +62,12 @@ bool z_max_endstop = false;
// Variables used by The Stepper Driver Interrupt
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_z,
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 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
@ -404,14 +405,14 @@ void isr() {
// The busy flag is set by the plan_get_current_block() call.
// current_block->busy = true;
trapezoid_generator_reset();
counter_x = -(current_block->step_event_count >> 1);
counter_y = counter_x;
counter_z = counter_x;
counter_e = counter_x;
step_events_completed = 0;
counter_x.wide = -(current_block->step_event_count.wide >> 1);
counter_y.wide = counter_x.wide;
counter_z.wide = counter_x.wide;
counter_e.wide = counter_x.wide;
step_events_completed.wide = 0;
#ifdef Z_LATE_ENABLE
if(current_block->steps_z > 0) {
if(current_block->steps_z.wide > 0) {
enable_z();
_NEXT_ISR(2000); //1ms wait
return;
@ -476,10 +477,10 @@ void isr() {
// Normal homing
x_min_endstop = (READ(X_MIN_PIN) != X_MIN_ENDSTOP_INVERTING);
#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];
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;
#endif
@ -499,10 +500,10 @@ void isr() {
// Normal homing
x_max_endstop = (READ(X_MAX_PIN) != X_MAX_ENDSTOP_INVERTING);
#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];
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;
#endif
@ -527,10 +528,10 @@ void isr() {
// Normal homing
y_min_endstop = (READ(Y_MIN_PIN) != Y_MIN_ENDSTOP_INVERTING);
#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];
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;
#endif
@ -548,10 +549,10 @@ void isr() {
// Normal homing
y_max_endstop = (READ(Y_MAX_PIN) != Y_MAX_ENDSTOP_INVERTING);
#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];
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;
#endif
@ -575,10 +576,10 @@ void isr() {
#else
z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING);
#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];
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;
#endif
@ -601,10 +602,10 @@ void isr() {
#else
z_max_endstop = (READ(Z_MAX_PIN) != Z_MAX_ENDSTOP_INVERTING);
#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];
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;
#endif
@ -625,7 +626,7 @@ void isr() {
if(z_min_endstop && old_z_min_endstop) {
endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
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;
}
@ -657,22 +658,22 @@ void isr() {
#endif //RP - returned, because missing characters
#ifdef LIN_ADVANCE
counter_e += current_block->steps_e;
if (counter_e > 0) {
counter_e -= current_block->step_event_count;
counter_e.wide += current_block->steps_e.wide;
if (counter_e.wide > 0) {
counter_e.wide -= current_block->step_event_count.wide;
count_position[E_AXIS] += count_direction[E_AXIS];
((out_bits&(1<<E_AXIS))!=0) ? --e_steps : ++e_steps;
}
#endif
counter_x += current_block->steps_x;
if (counter_x > 0) {
counter_x.wide += current_block->steps_x.wide;
if (counter_x.wide > 0) {
WRITE_NC(X_STEP_PIN, !INVERT_X_STEP_PIN);
LastStepMask |= X_AXIS_MASK;
#ifdef DEBUG_XSTEP_DUP_PIN
WRITE_NC(DEBUG_XSTEP_DUP_PIN,!INVERT_X_STEP_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];
WRITE_NC(X_STEP_PIN, INVERT_X_STEP_PIN);
#ifdef DEBUG_XSTEP_DUP_PIN
@ -680,8 +681,8 @@ void isr() {
#endif //DEBUG_XSTEP_DUP_PIN
}
counter_y += current_block->steps_y;
if (counter_y > 0) {
counter_y.wide += current_block->steps_y.wide;
if (counter_y.wide > 0) {
WRITE_NC(Y_STEP_PIN, !INVERT_Y_STEP_PIN);
LastStepMask |= Y_AXIS_MASK;
#ifdef DEBUG_YSTEP_DUP_PIN
@ -692,7 +693,7 @@ void isr() {
WRITE_NC(Y2_STEP_PIN, !INVERT_Y_STEP_PIN);
#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];
WRITE_NC(Y_STEP_PIN, INVERT_Y_STEP_PIN);
#ifdef DEBUG_YSTEP_DUP_PIN
@ -704,15 +705,15 @@ void isr() {
#endif
}
counter_z += current_block->steps_z;
if (counter_z > 0) {
counter_z.wide += current_block->steps_z.wide;
if (counter_z.wide > 0) {
WRITE_NC(Z_STEP_PIN, !INVERT_Z_STEP_PIN);
LastStepMask |= Z_AXIS_MASK;
#ifdef Z_DUAL_STEPPER_DRIVERS
WRITE_NC(Z2_STEP_PIN, !INVERT_Z_STEP_PIN);
#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];
WRITE_NC(Z_STEP_PIN, INVERT_Z_STEP_PIN);
@ -722,10 +723,10 @@ void isr() {
}
#ifndef LIN_ADVANCE
counter_e += current_block->steps_e;
if (counter_e > 0) {
counter_e.wide += current_block->steps_e.wide;
if (counter_e.wide > 0) {
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];
WRITE(E0_STEP_PIN, INVERT_E_STEP_PIN);
#ifdef PAT9125
@ -734,8 +735,8 @@ void isr() {
}
#endif
step_events_completed += 1;
if(step_events_completed >= current_block->step_event_count) break;
++ step_events_completed.wide;
if(step_events_completed.wide >= current_block->step_event_count.wide) break;
}
#ifdef LIN_ADVANCE
if (current_block->use_advance_lead) {
@ -750,7 +751,7 @@ void isr() {
// Calculare new timer value
unsigned short timer;
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
MultiU24X24toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate);
acc_step_rate += current_block->initial_rate;
@ -771,7 +772,7 @@ void isr() {
eISR_Rate = ADV_RATE(timer, step_loops);
#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);
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 (step_events_completed >= current_block->step_event_count) {
if (step_events_completed.wide >= current_block->step_event_count.wide) {
#ifdef PAT9125
fsensor_st_block_chunk(current_block, fsensor_counter);