Merge pull request #10614 from thinkyhead/bf2_synced_planner_set_position

[2.0.x] Improve sync of planner / stepper position, asynchronous G92
This commit is contained in:
Scott Lahteine 2018-05-06 03:10:43 -05:00 committed by GitHub
commit f30241bda5
Signed by: GitHub
GPG key ID: 4AEE18F83AFDEB23
14 changed files with 159 additions and 123 deletions

View file

@ -415,9 +415,9 @@ void I2CPositionEncoder::calibrate_steps_mm(const uint8_t iter) {
startCoord[encoderAxis] = startDistance; startCoord[encoderAxis] = startDistance;
endCoord[encoderAxis] = endDistance; endCoord[encoderAxis] = endDistance;
LOOP_L_N(i, iter) {
stepper.synchronize(); stepper.synchronize();
LOOP_L_N(i, iter) {
planner.buffer_line(startCoord[X_AXIS], startCoord[Y_AXIS], startCoord[Z_AXIS], planner.buffer_line(startCoord[X_AXIS], startCoord[Y_AXIS], startCoord[Z_AXIS],
stepper.get_axis_position_mm(E_AXIS), feedrate, 0); stepper.get_axis_position_mm(E_AXIS), feedrate, 0);
stepper.synchronize(); stepper.synchronize();

View file

@ -108,7 +108,7 @@ void FWRetract::retract(const bool retracting
// G11 priority to recover the long retract if activated // G11 priority to recover the long retract if activated
if (!retracting) swapping = retracted_swap[active_extruder]; if (!retracting) swapping = retracted_swap[active_extruder];
#else #else
const bool swapping = false; constexpr bool swapping = false;
#endif #endif
/* // debugging /* // debugging
@ -118,62 +118,57 @@ void FWRetract::retract(const bool retracting
for (uint8_t i = 0; i < EXTRUDERS; ++i) { for (uint8_t i = 0; i < EXTRUDERS; ++i) {
SERIAL_ECHOPAIR("retracted[", i); SERIAL_ECHOPAIR("retracted[", i);
SERIAL_ECHOLNPAIR("] ", retracted[i]); SERIAL_ECHOLNPAIR("] ", retracted[i]);
#if EXTRUDERS > 1
SERIAL_ECHOPAIR("retracted_swap[", i); SERIAL_ECHOPAIR("retracted_swap[", i);
SERIAL_ECHOLNPAIR("] ", retracted_swap[i]); SERIAL_ECHOLNPAIR("] ", retracted_swap[i]);
#endif
} }
SERIAL_ECHOLNPAIR("current_position[z] ", current_position[Z_AXIS]); SERIAL_ECHOLNPAIR("current_position[z] ", current_position[Z_AXIS]);
SERIAL_ECHOLNPAIR("current_position[e] ", current_position[E_AXIS]); SERIAL_ECHOLNPAIR("current_position[e] ", current_position[E_AXIS]);
SERIAL_ECHOLNPAIR("hop_amount ", hop_amount); SERIAL_ECHOLNPAIR("hop_amount ", hop_amount);
//*/ //*/
const float old_feedrate_mm_s = feedrate_mm_s; const float old_feedrate_mm_s = feedrate_mm_s,
renormalize = RECIPROCAL(planner.e_factor[active_extruder]),
base_retract = swapping ? swap_retract_length : retract_length,
old_z = current_position[Z_AXIS],
old_e = current_position[E_AXIS];
// The current position will be the destination for E and Z moves // The current position will be the destination for E and Z moves
set_destination_from_current(); set_destination_from_current();
stepper.synchronize(); // Wait for buffered moves to complete
const float renormalize = 1.0 / planner.e_factor[active_extruder];
if (retracting) { if (retracting) {
// Retract by moving from a faux E position back to the current E position // Retract by moving from a faux E position back to the current E position
feedrate_mm_s = retract_feedrate_mm_s; feedrate_mm_s = retract_feedrate_mm_s;
current_position[E_AXIS] += (swapping ? swap_retract_length : retract_length) * renormalize; destination[E_AXIS] -= base_retract * renormalize;
sync_plan_position_e();
prepare_move_to_destination(); // set_current_to_destination prepare_move_to_destination(); // set_current_to_destination
// Is a Z hop set, and has the hop not yet been done? // Is a Z hop set, and has the hop not yet been done?
// No double zlifting
// Feedrate to the max
if (retract_zlift > 0.01 && !hop_amount) { // Apply hop only once if (retract_zlift > 0.01 && !hop_amount) { // Apply hop only once
const float old_z = current_position[Z_AXIS];
hop_amount += retract_zlift; // Add to the hop total (again, only once) hop_amount += retract_zlift; // Add to the hop total (again, only once)
destination[Z_AXIS] += retract_zlift; // Raise Z by the zlift (M207 Z) amount destination[Z_AXIS] += retract_zlift; // Raise Z by the zlift (M207 Z) amount
feedrate_mm_s = planner.max_feedrate_mm_s[Z_AXIS]; // Maximum Z feedrate feedrate_mm_s = planner.max_feedrate_mm_s[Z_AXIS]; // Maximum Z feedrate
prepare_move_to_destination(); // Raise up, set_current_to_destination prepare_move_to_destination(); // Raise up, set_current_to_destination
current_position[Z_AXIS] = old_z; // Spoof the Z position in the planner
SYNC_PLAN_POSITION_KINEMATIC();
} }
} }
else { else {
// If a hop was done and Z hasn't changed, undo the Z hop // If a hop was done and Z hasn't changed, undo the Z hop
if (hop_amount) { if (hop_amount) {
current_position[Z_AXIS] += hop_amount; // Set actual Z (due to the prior hop) destination[Z_AXIS] -= hop_amount; // Move back down by the total hop amount
SYNC_PLAN_POSITION_KINEMATIC(); // Spoof the Z position in the planner
feedrate_mm_s = planner.max_feedrate_mm_s[Z_AXIS]; // Z feedrate to max feedrate_mm_s = planner.max_feedrate_mm_s[Z_AXIS]; // Z feedrate to max
prepare_move_to_destination(); // Lower Z, set_current_to_destination prepare_move_to_destination(); // Lower Z, set_current_to_destination
hop_amount = 0.0; // Clear the hop amount hop_amount = 0.0; // Clear the hop amount
} }
// A retract multiplier has been added here to get faster swap recovery destination[E_AXIS] += (base_retract + (swapping ? swap_retract_recover_length : retract_recover_length)) * renormalize;
feedrate_mm_s = swapping ? swap_retract_recover_feedrate_mm_s : retract_recover_feedrate_mm_s; feedrate_mm_s = swapping ? swap_retract_recover_feedrate_mm_s : retract_recover_feedrate_mm_s;
current_position[E_AXIS] -= (swapping ? swap_retract_length + swap_retract_recover_length
: retract_length + retract_recover_length) * renormalize;
sync_plan_position_e();
prepare_move_to_destination(); // Recover E, set_current_to_destination prepare_move_to_destination(); // Recover E, set_current_to_destination
} }
feedrate_mm_s = old_feedrate_mm_s; // Restore original feedrate feedrate_mm_s = old_feedrate_mm_s; // Restore original feedrate
current_position[Z_AXIS] = old_z; // Restore Z and E positions
current_position[E_AXIS] = old_e;
SYNC_PLAN_POSITION_KINEMATIC(); // As if the move never took place
retracted[active_extruder] = retracting; // Active extruder now retracted / recovered retracted[active_extruder] = retracting; // Active extruder now retracted / recovered
@ -189,8 +184,10 @@ void FWRetract::retract(const bool retracting
for (uint8_t i = 0; i < EXTRUDERS; ++i) { for (uint8_t i = 0; i < EXTRUDERS; ++i) {
SERIAL_ECHOPAIR("retracted[", i); SERIAL_ECHOPAIR("retracted[", i);
SERIAL_ECHOLNPAIR("] ", retracted[i]); SERIAL_ECHOLNPAIR("] ", retracted[i]);
#if EXTRUDERS > 1
SERIAL_ECHOPAIR("retracted_swap[", i); SERIAL_ECHOPAIR("retracted_swap[", i);
SERIAL_ECHOLNPAIR("] ", retracted_swap[i]); SERIAL_ECHOLNPAIR("] ", retracted_swap[i]);
#endif
} }
SERIAL_ECHOLNPAIR("current_position[z] ", current_position[Z_AXIS]); SERIAL_ECHOLNPAIR("current_position[z] ", current_position[Z_AXIS]);
SERIAL_ECHOLNPAIR("current_position[e] ", current_position[E_AXIS]); SERIAL_ECHOLNPAIR("current_position[e] ", current_position[E_AXIS]);

View file

@ -121,8 +121,8 @@ static void do_pause_e_move(const float &length, const float &fr) {
set_destination_from_current(); set_destination_from_current();
destination[E_AXIS] += length / planner.e_factor[active_extruder]; destination[E_AXIS] += length / planner.e_factor[active_extruder];
planner.buffer_line_kinematic(destination, fr, active_extruder); planner.buffer_line_kinematic(destination, fr, active_extruder);
stepper.synchronize();
set_current_from_destination(); set_current_from_destination();
stepper.synchronize();
} }
/** /**
@ -366,12 +366,12 @@ bool pause_print(const float &retract, const point_t &park_point, const float &u
#endif #endif
print_job_timer.pause(); print_job_timer.pause();
// Wait for synchronize steppers
stepper.synchronize();
// Save current position // Save current position
COPY(resume_position, current_position); COPY(resume_position, current_position);
// Wait for buffered blocks to complete
stepper.synchronize();
// Initial retract before move to filament change position // Initial retract before move to filament change position
if (retract && thermalManager.hotEnoughToExtrude(active_extruder)) if (retract && thermalManager.hotEnoughToExtrude(active_extruder))
do_pause_e_move(retract, PAUSE_PARK_RETRACT_FEEDRATE); do_pause_e_move(retract, PAUSE_PARK_RETRACT_FEEDRATE);

View file

@ -240,8 +240,6 @@ void move_to(const float &rx, const float &ry, const float &z, const float &e_de
destination[E_AXIS] = current_position[E_AXIS]; destination[E_AXIS] = current_position[E_AXIS];
G26_line_to_destination(feed_value); G26_line_to_destination(feed_value);
stepper.synchronize();
set_destination_from_current(); set_destination_from_current();
} }
@ -256,8 +254,6 @@ void move_to(const float &rx, const float &ry, const float &z, const float &e_de
destination[E_AXIS] += e_delta; destination[E_AXIS] += e_delta;
G26_line_to_destination(feed_value); G26_line_to_destination(feed_value);
stepper.synchronize();
set_destination_from_current(); set_destination_from_current();
} }
@ -499,13 +495,11 @@ inline bool prime_nozzle() {
if (Total_Prime >= EXTRUDE_MAXLENGTH) return G26_ERR; if (Total_Prime >= EXTRUDE_MAXLENGTH) return G26_ERR;
#endif #endif
G26_line_to_destination(planner.max_feedrate_mm_s[E_AXIS] / 15.0); G26_line_to_destination(planner.max_feedrate_mm_s[E_AXIS] / 15.0);
set_destination_from_current();
stepper.synchronize(); // Without this synchronize, the purge is more consistent, stepper.synchronize(); // Without this synchronize, the purge is more consistent,
// but because the planner has a buffer, we won't be able // but because the planner has a buffer, we won't be able
// to stop as quickly. So we put up with the less smooth // to stop as quickly. So we put up with the less smooth
// action to give the user a more responsive 'Stop'. // action to give the user a more responsive 'Stop'.
set_destination_from_current();
idle();
} }
wait_for_release(); wait_for_release();
@ -526,7 +520,6 @@ inline bool prime_nozzle() {
set_destination_from_current(); set_destination_from_current();
destination[E_AXIS] += g26_prime_length; destination[E_AXIS] += g26_prime_length;
G26_line_to_destination(planner.max_feedrate_mm_s[E_AXIS] / 15.0); G26_line_to_destination(planner.max_feedrate_mm_s[E_AXIS] / 15.0);
stepper.synchronize();
set_destination_from_current(); set_destination_from_current();
retract_filament(destination); retract_filament(destination);
} }
@ -700,7 +693,6 @@ void GcodeSuite::G26() {
if (current_position[Z_AXIS] < Z_CLEARANCE_BETWEEN_PROBES) { if (current_position[Z_AXIS] < Z_CLEARANCE_BETWEEN_PROBES) {
do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES); do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES);
stepper.synchronize();
set_current_from_destination(); set_current_from_destination();
} }

View file

@ -949,8 +949,8 @@ void GcodeSuite::G29() {
#if ENABLED(DEBUG_LEVELING_FEATURE) #if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPAIR("Z Probe End Script: ", Z_PROBE_END_SCRIPT); if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPAIR("Z Probe End Script: ", Z_PROBE_END_SCRIPT);
#endif #endif
enqueue_and_echo_commands_P(PSTR(Z_PROBE_END_SCRIPT));
stepper.synchronize(); stepper.synchronize();
enqueue_and_echo_commands_P(PSTR(Z_PROBE_END_SCRIPT));
#endif #endif
// Auto Bed Leveling is complete! Enable if possible. // Auto Bed Leveling is complete! Enable if possible.

View file

@ -108,7 +108,6 @@ void GcodeSuite::M81() {
safe_delay(1000); // Wait 1 second before switching off safe_delay(1000); // Wait 1 second before switching off
#if HAS_SUICIDE #if HAS_SUICIDE
stepper.synchronize();
suicide(); suicide();
#elif HAS_POWER_SWITCH #elif HAS_POWER_SWITCH
PSU_OFF(); PSU_OFF();

View file

@ -33,8 +33,6 @@
*/ */
void GcodeSuite::G92() { void GcodeSuite::G92() {
stepper.synchronize();
#if ENABLED(CNC_COORDINATE_SYSTEMS) #if ENABLED(CNC_COORDINATE_SYSTEMS)
switch (parser.subcode) { switch (parser.subcode) {
case 1: case 1:
@ -94,10 +92,8 @@ void GcodeSuite::G92() {
COPY(coordinate_system[active_coordinate_system], position_shift); COPY(coordinate_system[active_coordinate_system], position_shift);
#endif #endif
if (didXYZ) if (didXYZ) SYNC_PLAN_POSITION_KINEMATIC();
SYNC_PLAN_POSITION_KINEMATIC(); else if (didE) sync_plan_position_e();
else if (didE)
sync_plan_position_e();
report_current_position(); report_current_position();
} }

View file

@ -43,8 +43,6 @@
void report_current_position_detail() { void report_current_position_detail() {
stepper.synchronize();
SERIAL_PROTOCOLPGM("\nLogical:"); SERIAL_PROTOCOLPGM("\nLogical:");
const float logical[XYZ] = { const float logical[XYZ] = {
LOGICAL_X_POSITION(current_position[X_AXIS]), LOGICAL_X_POSITION(current_position[X_AXIS]),
@ -79,6 +77,8 @@
report_xyz(delta); report_xyz(delta);
#endif #endif
stepper.synchronize();
SERIAL_PROTOCOLPGM("Stepper:"); SERIAL_PROTOCOLPGM("Stepper:");
LOOP_XYZE(i) { LOOP_XYZE(i) {
SERIAL_CHAR(' '); SERIAL_CHAR(' ');

View file

@ -58,6 +58,8 @@ void GcodeSuite::M0_M1() {
const bool has_message = !hasP && !hasS && args && *args; const bool has_message = !hasP && !hasS && args && *args;
stepper.synchronize();
#if ENABLED(ULTIPANEL) #if ENABLED(ULTIPANEL)
if (has_message) if (has_message)
@ -81,8 +83,6 @@ void GcodeSuite::M0_M1() {
KEEPALIVE_STATE(PAUSED_FOR_USER); KEEPALIVE_STATE(PAUSED_FOR_USER);
wait_for_user = true; wait_for_user = true;
stepper.synchronize();
if (ms > 0) { if (ms > 0) {
ms += millis(); // wait until this time for a click ms += millis(); // wait until this time for a click
while (PENDING(millis(), ms) && wait_for_user) idle(); while (PENDING(millis(), ms) && wait_for_user) idle();

View file

@ -396,13 +396,13 @@ void do_blocking_move_to(const float rx, const float ry, const float rz, const f
#endif #endif
stepper.synchronize();
feedrate_mm_s = old_feedrate_mm_s; feedrate_mm_s = old_feedrate_mm_s;
#if ENABLED(DEBUG_LEVELING_FEATURE) #if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("<<< do_blocking_move_to"); if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("<<< do_blocking_move_to");
#endif #endif
stepper.synchronize();
} }
void do_blocking_move_to_x(const float &rx, const float &fr_mm_s/*=0.0*/) { void do_blocking_move_to_x(const float &rx, const float &fr_mm_s/*=0.0*/) {
do_blocking_move_to(rx, current_position[Y_AXIS], current_position[Z_AXIS], fr_mm_s); do_blocking_move_to(rx, current_position[Y_AXIS], current_position[Z_AXIS], fr_mm_s);
@ -881,8 +881,8 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS],
planner.max_feedrate_mm_s[X_AXIS], 1 planner.max_feedrate_mm_s[X_AXIS], 1
); );
SYNC_PLAN_POSITION_KINEMATIC();
stepper.synchronize(); stepper.synchronize();
SYNC_PLAN_POSITION_KINEMATIC();
extruder_duplication_enabled = true; extruder_duplication_enabled = true;
active_extruder_parked = false; active_extruder_parked = false;
#if ENABLED(DEBUG_LEVELING_FEATURE) #if ENABLED(DEBUG_LEVELING_FEATURE)
@ -1106,7 +1106,7 @@ static void do_homing_move(const AxisEnum axis, const float distance, const floa
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], current_position[E_AXIS], fr_mm_s ? fr_mm_s : homing_feedrate(axis), active_extruder); planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], current_position[E_AXIS], fr_mm_s ? fr_mm_s : homing_feedrate(axis), active_extruder);
#else #else
sync_plan_position(); sync_plan_position();
current_position[axis] = distance; current_position[axis] = distance; // Set delta/cartesian axes directly
planner.buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], fr_mm_s ? fr_mm_s : homing_feedrate(axis), active_extruder); planner.buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], fr_mm_s ? fr_mm_s : homing_feedrate(axis), active_extruder);
#endif #endif

View file

@ -1382,15 +1382,9 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
const float esteps_float = de * e_factor[extruder]; const float esteps_float = de * e_factor[extruder];
const int32_t esteps = abs(esteps_float) + 0.5; const int32_t esteps = abs(esteps_float) + 0.5;
// Calculate the buffer head after we push this byte // Wait for the next available block
const uint8_t next_buffer_head = next_block_index(block_buffer_head); uint8_t next_buffer_head;
block_t * const block = get_next_free_block(next_buffer_head);
// If the buffer is full: good! That means we are well ahead of the robot.
// Rest here until there is room in the buffer.
while (block_buffer_tail == next_buffer_head) idle();
// Prepare to set up new block
block_t* block = &block_buffer[block_buffer_head];
// Clear all flags, including the "busy" bit // Clear all flags, including the "busy" bit
block->flag = 0x00; block->flag = 0x00;
@ -2032,6 +2026,26 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
} // _buffer_steps() } // _buffer_steps()
/**
* Planner::buffer_sync_block
* Add a block to the buffer that just updates the position
*/
void Planner::buffer_sync_block() {
// Wait for the next available block
uint8_t next_buffer_head;
block_t * const block = get_next_free_block(next_buffer_head);
block->steps[A_AXIS] = position[A_AXIS];
block->steps[B_AXIS] = position[B_AXIS];
block->steps[C_AXIS] = position[C_AXIS];
block->steps[E_AXIS] = position[E_AXIS];
block->flag = BLOCK_FLAG_SYNC_POSITION;
block_buffer_head = next_buffer_head;
stepper.wake_up();
} // buffer_sync_block()
/** /**
* Planner::buffer_segment * Planner::buffer_segment
* *
@ -2160,19 +2174,19 @@ void Planner::_set_position_mm(const float &a, const float &b, const float &c, c
#else #else
#define _EINDEX E_AXIS #define _EINDEX E_AXIS
#endif #endif
const int32_t na = position[A_AXIS] = LROUND(a * axis_steps_per_mm[A_AXIS]), position[A_AXIS] = LROUND(a * axis_steps_per_mm[A_AXIS]),
nb = position[B_AXIS] = LROUND(b * axis_steps_per_mm[B_AXIS]), position[B_AXIS] = LROUND(b * axis_steps_per_mm[B_AXIS]),
nc = position[C_AXIS] = LROUND(c * axis_steps_per_mm[C_AXIS]), position[C_AXIS] = LROUND(c * axis_steps_per_mm[C_AXIS]),
ne = position[E_AXIS] = LROUND(e * axis_steps_per_mm[_EINDEX]); position[E_AXIS] = LROUND(e * axis_steps_per_mm[_EINDEX]);
#if HAS_POSITION_FLOAT #if HAS_POSITION_FLOAT
position_float[X_AXIS] = a; position_float[A_AXIS] = a;
position_float[Y_AXIS] = b; position_float[B_AXIS] = b;
position_float[Z_AXIS] = c; position_float[C_AXIS] = c;
position_float[E_AXIS] = e; position_float[E_AXIS] = e;
#endif #endif
stepper.set_position(na, nb, nc, 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.
ZERO(previous_speed); ZERO(previous_speed);
buffer_sync_block();
} }
void Planner::set_position_mm_kinematic(const float (&cart)[XYZE]) { void Planner::set_position_mm_kinematic(const float (&cart)[XYZE]) {
@ -2220,23 +2234,23 @@ void Planner::set_position_mm(const AxisEnum axis, const float &v) {
#if HAS_POSITION_FLOAT #if HAS_POSITION_FLOAT
position_float[axis] = v; position_float[axis] = v;
#endif #endif
stepper.set_position(axis, position[axis]);
previous_speed[axis] = 0.0; previous_speed[axis] = 0.0;
buffer_sync_block();
} }
// Recalculate the steps/s^2 acceleration rates, based on the mm/s^2 // Recalculate the steps/s^2 acceleration rates, based on the mm/s^2
void Planner::reset_acceleration_rates() { void Planner::reset_acceleration_rates() {
#if ENABLED(DISTINCT_E_FACTORS) #if ENABLED(DISTINCT_E_FACTORS)
#define HIGHEST_CONDITION (i < E_AXIS || i == E_AXIS + active_extruder) #define AXIS_CONDITION (i < E_AXIS || i == E_AXIS + active_extruder)
#else #else
#define HIGHEST_CONDITION true #define AXIS_CONDITION true
#endif #endif
uint32_t highest_rate = 1; uint32_t highest_rate = 1;
LOOP_XYZE_N(i) { LOOP_XYZE_N(i) {
max_acceleration_steps_per_s2[i] = max_acceleration_mm_per_s2[i] * axis_steps_per_mm[i]; max_acceleration_steps_per_s2[i] = max_acceleration_mm_per_s2[i] * axis_steps_per_mm[i];
if (HIGHEST_CONDITION) NOLESS(highest_rate, max_acceleration_steps_per_s2[i]); if (AXIS_CONDITION) NOLESS(highest_rate, max_acceleration_steps_per_s2[i]);
} }
cutoff_long = 4294967295UL / highest_rate; cutoff_long = 4294967295UL / highest_rate; // 0xFFFFFFFFUL
} }
// Recalculate position, steps_to_mm if axis_steps_per_mm changes! // Recalculate position, steps_to_mm if axis_steps_per_mm changes!

View file

@ -57,14 +57,18 @@ enum BlockFlagBit : char {
BLOCK_BIT_BUSY, BLOCK_BIT_BUSY,
// The block is segment 2+ of a longer move // The block is segment 2+ of a longer move
BLOCK_BIT_CONTINUED BLOCK_BIT_CONTINUED,
// Sync the stepper counts from the block
BLOCK_BIT_SYNC_POSITION
}; };
enum BlockFlag : char { enum BlockFlag : char {
BLOCK_FLAG_RECALCULATE = _BV(BLOCK_BIT_RECALCULATE), BLOCK_FLAG_RECALCULATE = _BV(BLOCK_BIT_RECALCULATE),
BLOCK_FLAG_NOMINAL_LENGTH = _BV(BLOCK_BIT_NOMINAL_LENGTH), BLOCK_FLAG_NOMINAL_LENGTH = _BV(BLOCK_BIT_NOMINAL_LENGTH),
BLOCK_FLAG_BUSY = _BV(BLOCK_BIT_BUSY), BLOCK_FLAG_BUSY = _BV(BLOCK_BIT_BUSY),
BLOCK_FLAG_CONTINUED = _BV(BLOCK_BIT_CONTINUED) BLOCK_FLAG_CONTINUED = _BV(BLOCK_BIT_CONTINUED),
BLOCK_FLAG_SYNC_POSITION = _BV(BLOCK_BIT_SYNC_POSITION)
}; };
/** /**
@ -422,6 +426,20 @@ class Planner {
#endif #endif
/**
* Planner::get_next_free_block
*
* - Get the next head index (passed by reference)
* - Wait for a space to open up in the planner
* - Return the head block
*/
FORCE_INLINE static block_t* get_next_free_block(uint8_t &next_buffer_head) {
next_buffer_head = next_block_index(block_buffer_head);
while (block_buffer_tail == next_buffer_head) idle(); // while (is_full)
return &block_buffer[block_buffer_head];
}
/** /**
* Planner::_buffer_steps * Planner::_buffer_steps
* *
@ -439,6 +457,12 @@ class Planner {
, float fr_mm_s, const uint8_t extruder, const float &millimeters=0.0 , float fr_mm_s, const uint8_t extruder, const float &millimeters=0.0
); );
/**
* Planner::buffer_sync_block
* Add a block to the buffer that just updates the position
*/
static void buffer_sync_block();
/** /**
* Planner::buffer_segment * Planner::buffer_segment
* *
@ -518,7 +542,7 @@ class Planner {
static void set_position_mm_kinematic(const float (&cart)[XYZE]); static void set_position_mm_kinematic(const float (&cart)[XYZE]);
static void set_position_mm(const AxisEnum axis, const float &v); static void set_position_mm(const AxisEnum axis, const float &v);
FORCE_INLINE static void set_z_position_mm(const float &z) { set_position_mm(Z_AXIS, z); } FORCE_INLINE static void set_z_position_mm(const float &z) { set_position_mm(Z_AXIS, z); }
FORCE_INLINE static void set_e_position_mm(const float &e) { set_position_mm(AxisEnum(E_AXIS), e); } FORCE_INLINE static void set_e_position_mm(const float &e) { set_position_mm(E_AXIS, e); }
/** /**
* Sync from the stepper positions. (e.g., after an interrupted move) * Sync from the stepper positions. (e.g., after an interrupted move)
@ -528,7 +552,7 @@ class Planner {
/** /**
* Does the buffer have any blocks queued? * Does the buffer have any blocks queued?
*/ */
static bool has_blocks_queued() { return (block_buffer_head != block_buffer_tail); } FORCE_INLINE static bool has_blocks_queued() { return (block_buffer_head != block_buffer_tail); }
/** /**
* "Discard" the block and "release" the memory. * "Discard" the block and "release" the memory.

View file

@ -109,7 +109,7 @@ int16_t Stepper::cleaning_buffer_counter = 0;
bool Stepper::locked_z_motor = false, Stepper::locked_z2_motor = false; bool Stepper::locked_z_motor = false, Stepper::locked_z2_motor = false;
#endif #endif
long Stepper::counter_X = 0, int32_t Stepper::counter_X = 0,
Stepper::counter_Y = 0, Stepper::counter_Y = 0,
Stepper::counter_Z = 0, Stepper::counter_Z = 0,
Stepper::counter_E = 0; Stepper::counter_E = 0;
@ -159,7 +159,7 @@ volatile int32_t Stepper::count_position[NUM_AXIS] = { 0 };
volatile signed char Stepper::count_direction[NUM_AXIS] = { 1, 1, 1, 1 }; volatile signed char Stepper::count_direction[NUM_AXIS] = { 1, 1, 1, 1 };
#if ENABLED(MIXING_EXTRUDER) #if ENABLED(MIXING_EXTRUDER)
long Stepper::counter_m[MIXING_STEPPERS]; int32_t Stepper::counter_m[MIXING_STEPPERS];
#endif #endif
uint8_t Stepper::step_loops, Stepper::step_loops_nominal; uint8_t Stepper::step_loops, Stepper::step_loops_nominal;
@ -169,7 +169,7 @@ hal_timer_t Stepper::OCR1A_nominal;
hal_timer_t Stepper::acc_step_rate; // needed for deceleration start point hal_timer_t Stepper::acc_step_rate; // needed for deceleration start point
#endif #endif
volatile long Stepper::endstops_trigsteps[XYZ]; volatile int32_t Stepper::endstops_trigsteps[XYZ];
#if ENABLED(X_DUAL_ENDSTOPS) || ENABLED(Y_DUAL_ENDSTOPS) || ENABLED(Z_DUAL_ENDSTOPS) #if ENABLED(X_DUAL_ENDSTOPS) || ENABLED(Y_DUAL_ENDSTOPS) || ENABLED(Z_DUAL_ENDSTOPS)
#define LOCKED_X_MOTOR locked_x_motor #define LOCKED_X_MOTOR locked_x_motor
@ -1217,6 +1217,16 @@ void Stepper::isr() {
// Anything in the buffer? // Anything in the buffer?
if ((current_block = planner.get_current_block())) { if ((current_block = planner.get_current_block())) {
// Sync block? Sync the stepper counts and return
while (TEST(current_block->flag, BLOCK_BIT_SYNC_POSITION)) {
_set_position(
current_block->steps[A_AXIS], current_block->steps[B_AXIS],
current_block->steps[C_AXIS], current_block->steps[E_AXIS]
);
planner.discard_current_block();
if (!(current_block = planner.get_current_block())) return;
}
// Initialize the trapezoid generator from the current block. // Initialize the trapezoid generator from the current block.
static int8_t last_extruder = -1; static int8_t last_extruder = -1;
@ -1976,12 +1986,7 @@ void Stepper::synchronize() { while (planner.has_blocks_queued() || cleaning_buf
* This allows get_axis_position_mm to correctly * This allows get_axis_position_mm to correctly
* derive the current XYZ position later on. * derive the current XYZ position later on.
*/ */
void Stepper::set_position(const long &a, const long &b, const long &c, const long &e) { void Stepper::_set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e) {
synchronize(); // Bad to set stepper counts in the middle of a move
CRITICAL_SECTION_START;
#if CORE_IS_XY #if CORE_IS_XY
// corexy positioning // corexy positioning
// 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
@ -2004,29 +2009,15 @@ void Stepper::set_position(const long &a, const long &b, const long &c, const lo
count_position[Y_AXIS] = b; count_position[Y_AXIS] = b;
count_position[Z_AXIS] = c; count_position[Z_AXIS] = c;
#endif #endif
count_position[E_AXIS] = e; count_position[E_AXIS] = e;
CRITICAL_SECTION_END;
}
void Stepper::set_position(const AxisEnum &axis, const long &v) {
CRITICAL_SECTION_START;
count_position[axis] = v;
CRITICAL_SECTION_END;
}
void Stepper::set_e_position(const long &e) {
CRITICAL_SECTION_START;
count_position[E_AXIS] = e;
CRITICAL_SECTION_END;
} }
/** /**
* Get a stepper's position in steps. * Get a stepper's position in steps.
*/ */
long Stepper::position(const AxisEnum axis) { int32_t Stepper::position(const AxisEnum axis) {
CRITICAL_SECTION_START; CRITICAL_SECTION_START;
const long count_pos = count_position[axis]; const int32_t count_pos = count_position[axis];
CRITICAL_SECTION_END; CRITICAL_SECTION_END;
return count_pos; return count_pos;
} }
@ -2095,7 +2086,7 @@ void Stepper::endstop_triggered(const AxisEnum axis) {
void Stepper::report_positions() { void Stepper::report_positions() {
CRITICAL_SECTION_START; CRITICAL_SECTION_START;
const long xpos = count_position[X_AXIS], const int32_t xpos = count_position[X_AXIS],
ypos = count_position[Y_AXIS], ypos = count_position[Y_AXIS],
zpos = count_position[Z_AXIS]; zpos = count_position[Z_AXIS];
CRITICAL_SECTION_END; CRITICAL_SECTION_END;

View file

@ -94,7 +94,7 @@ class Stepper {
#endif #endif
// Counter variables for the Bresenham line tracer // Counter variables for the Bresenham line tracer
static long counter_X, counter_Y, counter_Z, counter_E; static int32_t counter_X, counter_Y, counter_Z, counter_E;
static volatile uint32_t step_events_completed; // The number of step events executed in the current block static volatile uint32_t step_events_completed; // The number of step events executed in the current block
#if ENABLED(BEZIER_JERK_CONTROL) #if ENABLED(BEZIER_JERK_CONTROL)
@ -137,8 +137,8 @@ class Stepper {
static hal_timer_t acc_step_rate; // needed for deceleration start point static hal_timer_t acc_step_rate; // needed for deceleration start point
#endif #endif
static volatile long endstops_trigsteps[XYZ]; static volatile int32_t endstops_trigsteps[XYZ];
static volatile long endstops_stepsTotal, endstops_stepsDone; static volatile int32_t endstops_stepsTotal, endstops_stepsDone;
// //
// Positions of stepper motors, in step units // Positions of stepper motors, in step units
@ -154,7 +154,7 @@ class Stepper {
// Mixing extruder mix counters // Mixing extruder mix counters
// //
#if ENABLED(MIXING_EXTRUDER) #if ENABLED(MIXING_EXTRUDER)
static long counter_m[MIXING_STEPPERS]; static int32_t counter_m[MIXING_STEPPERS];
#define MIXING_STEPPERS_LOOP(VAR) \ #define MIXING_STEPPERS_LOOP(VAR) \
for (uint8_t VAR = 0; VAR < MIXING_STEPPERS; VAR++) \ for (uint8_t VAR = 0; VAR < MIXING_STEPPERS; VAR++) \
if (current_block->mix_event_count[VAR]) if (current_block->mix_event_count[VAR])
@ -191,9 +191,32 @@ class Stepper {
// //
// Set the current position in steps // Set the current position in steps
// //
static void set_position(const long &a, const long &b, const long &c, const long &e); static void _set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e);
static void set_position(const AxisEnum &a, const long &v);
static void set_e_position(const long &e); FORCE_INLINE static void _set_position(const AxisEnum a, const int32_t &v) { count_position[a] = v; }
FORCE_INLINE static void set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e) {
synchronize();
CRITICAL_SECTION_START;
_set_position(a, b, c, e);
CRITICAL_SECTION_END;
}
static void set_position(const AxisEnum a, const int32_t &v) {
synchronize();
CRITICAL_SECTION_START;
count_position[a] = v;
CRITICAL_SECTION_END;
}
FORCE_INLINE static void _set_e_position(const int32_t &e) { count_position[E_AXIS] = e; }
static void set_e_position(const int32_t &e) {
synchronize();
CRITICAL_SECTION_START;
count_position[E_AXIS] = e;
CRITICAL_SECTION_END;
}
// //
// Set direction bits for all steppers // Set direction bits for all steppers
@ -203,7 +226,7 @@ class Stepper {
// //
// Get the position of a stepper, in steps // Get the position of a stepper, in steps
// //
static long position(const AxisEnum axis); static int32_t position(const AxisEnum axis);
// //
// Report the positions of the steppers, in steps // Report the positions of the steppers, in steps