andrey
/
Marlin_FB4S
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Improve sync of stepper positions

pull/1/head
Scott Lahteine 7 years ago
parent
0c23792344
commit
af1950a63e
5 changed files with 103 additions and 55 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 8
      Marlin/src/gcode/geometry/G92.cpp
  2. 58
      Marlin/src/module/planner.cpp
  3. 32
      Marlin/src/module/planner.h
  4. 31
      Marlin/src/module/stepper.cpp
  5. 29
      Marlin/src/module/stepper.h

8
Marlin/src/gcode/geometry/G92.cpp
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();
} }

58
Marlin/src/module/planner.cpp
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!

32
Marlin/src/module/planner.h
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.

31
Marlin/src/module/stepper.cpp
View File

@ -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 int32_t &a, const int32_t &b, const int32_t &c, const int32_t &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,21 +2009,7 @@ void Stepper::set_position(const int32_t &a, const int32_t &b, const int32_t &c,
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;
CRITICAL_SECTION_END;
}
void Stepper::set_position(const AxisEnum &axis, const int32_t &v) {
CRITICAL_SECTION_START;
count_position[axis] = v;
CRITICAL_SECTION_END;
}
void Stepper::set_e_position(const int32_t &e) {
CRITICAL_SECTION_START;
count_position[E_AXIS] = e; count_position[E_AXIS] = e;
CRITICAL_SECTION_END;
} }
/** /**

29
Marlin/src/module/stepper.h
View File

@ -191,9 +191,32 @@ class Stepper {
// //
// Set the current position in steps // Set the current position in steps
// //
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 int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e);
static void set_position(const AxisEnum &a, const int32_t &v);
static void set_e_position(const int32_t &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

Write Preview
Loading…
Cancel
Save