MPCNC/Firmware2
1
0
Fork 0
Code Issues 1 Pull Requests Releases Wiki Activity

Improve sync of stepper positions

Browse Source
This commit is contained in:
Scott Lahteine 2018-05-03 17:45:13 -05:00
parent 0c23792344
commit af1950a63e
5 changed files with 103 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
Marlin/src/gcode/geometry/G92.cpp
View File

@ -33,8 +33,6 @@
*/
void GcodeSuite::G92() {
stepper.synchronize();
#if ENABLED(CNC_COORDINATE_SYSTEMS)
switch (parser.subcode) {
case 1:
@ -94,10 +92,8 @@ void GcodeSuite::G92() {
COPY(coordinate_system[active_coordinate_system], position_shift);
#endif
if (didXYZ)
SYNC_PLAN_POSITION_KINEMATIC();
else if (didE)
sync_plan_position_e();
if (didXYZ) SYNC_PLAN_POSITION_KINEMATIC();
else if (didE) sync_plan_position_e();
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 int32_t esteps = abs(esteps_float) + 0.5;
// Calculate the buffer head after we push this byte
const uint8_t next_buffer_head = next_block_index(block_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];
// Wait for the next available block
uint8_t next_buffer_head;
block_t * const block = get_next_free_block(next_buffer_head);
// Clear all flags, including the "busy" bit
block->flag = 0x00;
@ -2032,6 +2026,26 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
} // _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
*
@ -2160,19 +2174,19 @@ void Planner::_set_position_mm(const float &a, const float &b, const float &c, c
#else
#define _EINDEX E_AXIS
#endif
const int32_t na = position[A_AXIS] = LROUND(a * axis_steps_per_mm[A_AXIS]),
nb = position[B_AXIS] = LROUND(b * axis_steps_per_mm[B_AXIS]),
nc = position[C_AXIS] = LROUND(c * axis_steps_per_mm[C_AXIS]),
ne = position[E_AXIS] = LROUND(e * axis_steps_per_mm[_EINDEX]);
position[A_AXIS] = LROUND(a * axis_steps_per_mm[A_AXIS]),
position[B_AXIS] = LROUND(b * axis_steps_per_mm[B_AXIS]),
position[C_AXIS] = LROUND(c * axis_steps_per_mm[C_AXIS]),
position[E_AXIS] = LROUND(e * axis_steps_per_mm[_EINDEX]);
#if HAS_POSITION_FLOAT
position_float[X_AXIS] = a;
position_float[Y_AXIS] = b;
position_float[Z_AXIS] = c;
position_float[A_AXIS] = a;
position_float[B_AXIS] = b;
position_float[C_AXIS] = c;
position_float[E_AXIS] = e;
#endif
stepper.set_position(na, nb, nc, ne);
previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.
ZERO(previous_speed);
buffer_sync_block();
}
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
position_float[axis] = v;
#endif
stepper.set_position(axis, position[axis]);
previous_speed[axis] = 0.0;
buffer_sync_block();
}
// Recalculate the steps/s^2 acceleration rates, based on the mm/s^2
void Planner::reset_acceleration_rates() {
#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
#define HIGHEST_CONDITION true
#define AXIS_CONDITION true
#endif
uint32_t highest_rate = 1;
LOOP_XYZE_N(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!

32
Marlin/src/module/planner.h
View File

@ -57,14 +57,18 @@ enum BlockFlagBit : char {
BLOCK_BIT_BUSY,
// 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 {
BLOCK_FLAG_RECALCULATE = _BV(BLOCK_BIT_RECALCULATE),
BLOCK_FLAG_NOMINAL_LENGTH = _BV(BLOCK_BIT_NOMINAL_LENGTH),
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
/**
* 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
*
@ -439,6 +457,12 @@ class Planner {
, 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
*
@ -518,7 +542,7 @@ class Planner {
static void set_position_mm_kinematic(const float (&cart)[XYZE]);
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_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)
@ -528,7 +552,7 @@ class Planner {
/**
* 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.

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

@ -1217,6 +1217,16 @@ void Stepper::isr() {
// Anything in the buffer?
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.
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
* 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) {
synchronize(); // Bad to set stepper counts in the middle of a move
CRITICAL_SECTION_START;
void Stepper::_set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e) {
#if CORE_IS_XY
// corexy positioning
// 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[Z_AXIS] = c;
#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;
CRITICAL_SECTION_END;
}
/**

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

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