Commit ce4d7e45 authored by MagoKimbra's avatar MagoKimbra

FIx step_loops

parent 4eb71d1f
...@@ -72,6 +72,7 @@ float max_z_jerk; ...@@ -72,6 +72,7 @@ float max_z_jerk;
float max_e_jerk[EXTRUDERS]; // mm/s - initial speed for extruder retract moves float max_e_jerk[EXTRUDERS]; // mm/s - initial speed for extruder retract moves
float mintravelfeedrate; float mintravelfeedrate;
unsigned long axis_steps_per_sqr_second[3 + EXTRUDERS]; unsigned long axis_steps_per_sqr_second[3 + EXTRUDERS];
uint8_t last_extruder;
#ifdef ENABLE_AUTO_BED_LEVELING #ifdef ENABLE_AUTO_BED_LEVELING
// Transform required to compensate for bed level // Transform required to compensate for bed level
...@@ -505,6 +506,17 @@ float junction_deviation = 0.1; ...@@ -505,6 +506,17 @@ float junction_deviation = 0.1;
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 + extruder]); target[E_AXIS] = lround(e * axis_steps_per_unit[E_AXIS + extruder]);
// If changing extruder have to recalculate current position based on
// the steps-per-mm value for the new extruder.
#if EXTRUDERS > 1
if(last_extruder != extruder && axis_steps_per_unit[E_AXIS + extruder] !=
axis_steps_per_unit[E_AXIS + last_extruder]) {
float factor = float(axis_steps_per_unit[E_AXIS + extruder]) /
float(axis_steps_per_unit[E_AXIS + last_extruder]);
position[E_AXIS] = lround(position[E_AXIS] * factor);
}
#endif
float dx = target[X_AXIS] - position[X_AXIS], float dx = target[X_AXIS] - position[X_AXIS],
dy = target[Y_AXIS] - position[Y_AXIS], dy = target[Y_AXIS] - position[Y_AXIS],
dz = target[Z_AXIS] - position[Z_AXIS], dz = target[Z_AXIS] - position[Z_AXIS],
...@@ -892,7 +904,7 @@ float junction_deviation = 0.1; ...@@ -892,7 +904,7 @@ float junction_deviation = 0.1;
xsteps = axis_steps_per_sqr_second[X_AXIS], xsteps = axis_steps_per_sqr_second[X_AXIS],
ysteps = axis_steps_per_sqr_second[Y_AXIS], ysteps = axis_steps_per_sqr_second[Y_AXIS],
zsteps = axis_steps_per_sqr_second[Z_AXIS], zsteps = axis_steps_per_sqr_second[Z_AXIS],
esteps = axis_steps_per_sqr_second[E_AXIS + active_extruder]; esteps = axis_steps_per_sqr_second[E_AXIS + extruder];
if ((float)acc_st * bsx / block->step_event_count > xsteps) acc_st = xsteps; if ((float)acc_st * bsx / block->step_event_count > xsteps) acc_st = xsteps;
if ((float)acc_st * bsy / block->step_event_count > ysteps) acc_st = ysteps; if ((float)acc_st * bsy / block->step_event_count > ysteps) acc_st = ysteps;
if ((float)acc_st * bsz / block->step_event_count > zsteps) acc_st = zsteps; if ((float)acc_st * bsz / block->step_event_count > zsteps) acc_st = zsteps;
...@@ -991,8 +1003,10 @@ float junction_deviation = 0.1; ...@@ -991,8 +1003,10 @@ float junction_deviation = 0.1;
block->nominal_length_flag = (block->nominal_speed <= v_allowable); block->nominal_length_flag = (block->nominal_speed <= v_allowable);
block->recalculate_flag = true; // Always calculate trapezoid for new block block->recalculate_flag = true; // Always calculate trapezoid for new block
calculate_trapezoid_for_block(block, block->entry_speed / block->nominal_speed, safe_speed / block->nominal_speed);
// Update previous path unit_vector and nominal speed // Update previous path unit_vector and nominal speed
for (int i = 0; i < NUM_AXIS; i++) previous_speed[i] = current_speed[i]; memcpy(previous_speed, current_speed, sizeof(previous_speed)); // previous_speed[] = current_speed[]
previous_nominal_speed = block->nominal_speed; previous_nominal_speed = block->nominal_speed;
#ifdef ADVANCE #ifdef ADVANCE
...@@ -1013,13 +1027,11 @@ float junction_deviation = 0.1; ...@@ -1013,13 +1027,11 @@ float junction_deviation = 0.1;
*/ */
#endif // ADVANCE #endif // ADVANCE
calculate_trapezoid_for_block(block, block->entry_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
for (int i = 0; i < NUM_AXIS; i++) position[i] = target[i]; memcpy(position, target, sizeof(target)); // position[] = target[]
planner_recalculate(); planner_recalculate();
...@@ -1056,6 +1068,7 @@ float junction_deviation = 0.1; ...@@ -1056,6 +1068,7 @@ float junction_deviation = 0.1;
ny = position[Y_AXIS] = lround(y * axis_steps_per_unit[Y_AXIS]), ny = position[Y_AXIS] = lround(y * axis_steps_per_unit[Y_AXIS]),
nz = position[Z_AXIS] = lround(z * axis_steps_per_unit[Z_AXIS]), nz = position[Z_AXIS] = lround(z * axis_steps_per_unit[Z_AXIS]),
ne = position[E_AXIS] = lround(e * axis_steps_per_unit[E_AXIS + active_extruder]); ne = position[E_AXIS] = lround(e * axis_steps_per_unit[E_AXIS + active_extruder]);
last_extruder = active_extruder;
st_set_position(nx, ny, nz, ne); 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.
...@@ -1064,6 +1077,7 @@ float junction_deviation = 0.1; ...@@ -1064,6 +1077,7 @@ float junction_deviation = 0.1;
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 + active_extruder]); position[E_AXIS] = lround(e * axis_steps_per_unit[E_AXIS + active_extruder]);
last_extruder = active_extruder;
st_set_e_position(position[E_AXIS]); st_set_e_position(position[E_AXIS]);
} }
......
...@@ -325,11 +325,18 @@ FORCE_INLINE unsigned short calc_timer(unsigned short step_rate) { ...@@ -325,11 +325,18 @@ FORCE_INLINE unsigned short calc_timer(unsigned short step_rate) {
step_rate = (step_rate >> 1) & 0x7fff; step_rate = (step_rate >> 1) & 0x7fff;
step_loops = 2; step_loops = 2;
} }
else else {
#endif
{
step_loops = 1; step_loops = 1;
} }
#else
if (step_rate > 10000) { // If steprate > 10kHz >> step 2 times
step_rate = (step_rate >> 1) & 0x7fff;
step_loops = 2;
}
else {
step_loops = 1;
}
#endif
if (step_rate < (F_CPU / 500000)) step_rate = (F_CPU / 500000); if (step_rate < (F_CPU / 500000)) step_rate = (F_CPU / 500000);
step_rate -= (F_CPU / 500000); // Correct for minimal speed step_rate -= (F_CPU / 500000); // Correct for minimal speed
...@@ -724,7 +731,7 @@ ISR(TIMER1_COMPA_vect) { ...@@ -724,7 +731,7 @@ ISR(TIMER1_COMPA_vect) {
step_rate = current_block->final_rate; step_rate = current_block->final_rate;
} }
else { else {
step_rate = acc_step_rate - step_rate; // Decelerate from aceleration end point. step_rate = acc_step_rate - step_rate; // Decelerate from acceleration end point.
} }
// lower limit // lower limit
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment