Use a, b, c instead of lx, ly, lz

master
Scott Lahteine 8 years ago
parent f8c2473a71
commit c5cac486f5

@ -6892,8 +6892,7 @@ inline void gcode_M503() {
KEEPALIVE_STATE(IN_HANDLER); KEEPALIVE_STATE(IN_HANDLER);
// Set extruder to saved position // Set extruder to saved position
current_position[E_AXIS] = lastpos[E_AXIS]; destination[E_AXIS] = current_position[E_AXIS] = lastpos[E_AXIS];
destination[E_AXIS] = lastpos[E_AXIS];
planner.set_e_position_mm(current_position[E_AXIS]); planner.set_e_position_mm(current_position[E_AXIS]);
#if IS_KINEMATIC #if IS_KINEMATIC

@ -594,13 +594,14 @@ void Planner::check_axes_activity() {
* Planner::_buffer_line * Planner::_buffer_line
* *
* Add a new linear movement to the buffer. * Add a new linear movement to the buffer.
* Not apply the leveling.
* *
* x,y,z,e - target position in mm * Leveling and kinematics should be applied ahead of calling this.
* fr_mm_s - (target) speed of the move *
* extruder - target extruder * a,b,c,e - target positions in mm or degrees
* fr_mm_s - (target) speed of the move
* extruder - target extruder
*/ */
void Planner::_buffer_line(const float &lx, const float &ly, const float &lz, const float &e, float fr_mm_s, const uint8_t extruder) { void Planner::_buffer_line(const float &a, const float &b, const float &c, const float &e, float fr_mm_s, const uint8_t extruder) {
// Calculate the buffer head after we push this byte // Calculate the buffer head after we push this byte
int next_buffer_head = next_block_index(block_buffer_head); int next_buffer_head = next_block_index(block_buffer_head);
@ -611,36 +612,36 @@ void Planner::_buffer_line(const float &lx, const float &ly, const float &lz, co
// The target position of the tool in absolute steps // The target position of the tool in absolute steps
// Calculate target position in absolute steps // Calculate target position in absolute steps
//this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow //this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
long target[NUM_AXIS] = { long target[XYZE] = {
lround(lx * axis_steps_per_mm[X_AXIS]), lround(a * axis_steps_per_mm[X_AXIS]),
lround(ly * axis_steps_per_mm[Y_AXIS]), lround(b * axis_steps_per_mm[Y_AXIS]),
lround(lz * axis_steps_per_mm[Z_AXIS]), lround(c * axis_steps_per_mm[Z_AXIS]),
lround(e * axis_steps_per_mm[E_AXIS]) lround(e * axis_steps_per_mm[E_AXIS])
}; };
long dx = target[X_AXIS] - position[X_AXIS], long da = target[X_AXIS] - position[X_AXIS],
dy = target[Y_AXIS] - position[Y_AXIS], db = target[Y_AXIS] - position[Y_AXIS],
dz = target[Z_AXIS] - position[Z_AXIS]; dc = target[Z_AXIS] - position[Z_AXIS];
/* /*
SERIAL_ECHOPAIR(" Planner FR:", fr_mm_s); SERIAL_ECHOPAIR(" Planner FR:", fr_mm_s);
SERIAL_CHAR(' '); SERIAL_CHAR(' ');
#if IS_KINEMATIC #if IS_KINEMATIC
SERIAL_ECHOPAIR("A:", lx); SERIAL_ECHOPAIR("A:", a);
SERIAL_ECHOPAIR(" (", dx); SERIAL_ECHOPAIR(" (", da);
SERIAL_ECHOPAIR(") B:", ly); SERIAL_ECHOPAIR(") B:", b);
#else #else
SERIAL_ECHOPAIR("X:", lx); SERIAL_ECHOPAIR("X:", a);
SERIAL_ECHOPAIR(" (", dx); SERIAL_ECHOPAIR(" (", da);
SERIAL_ECHOPAIR(") Y:", ly); SERIAL_ECHOPAIR(") Y:", b);
#endif #endif
SERIAL_ECHOPAIR(" (", dy); SERIAL_ECHOPAIR(" (", db);
#if ENABLED(DELTA) #if ENABLED(DELTA)
SERIAL_ECHOPAIR(") C:", lz); SERIAL_ECHOPAIR(") C:", c);
#else #else
SERIAL_ECHOPAIR(") Z:", lz); SERIAL_ECHOPAIR(") Z:", c);
#endif #endif
SERIAL_ECHOPAIR(" (", dz); SERIAL_ECHOPAIR(" (", dc);
SERIAL_CHAR(')'); SERIAL_CHAR(')');
SERIAL_EOL; SERIAL_EOL;
//*/ //*/
@ -679,24 +680,24 @@ void Planner::_buffer_line(const float &lx, const float &ly, const float &lz, co
#if ENABLED(COREXY) #if ENABLED(COREXY)
// corexy planning // corexy planning
// 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
block->steps[A_AXIS] = labs(dx + dy); block->steps[A_AXIS] = labs(da + db);
block->steps[B_AXIS] = labs(dx - dy); block->steps[B_AXIS] = labs(da - db);
block->steps[Z_AXIS] = labs(dz); block->steps[Z_AXIS] = labs(dc);
#elif ENABLED(COREXZ) #elif ENABLED(COREXZ)
// corexz planning // corexz planning
block->steps[A_AXIS] = labs(dx + dz); block->steps[A_AXIS] = labs(da + dc);
block->steps[Y_AXIS] = labs(dy); block->steps[Y_AXIS] = labs(db);
block->steps[C_AXIS] = labs(dx - dz); block->steps[C_AXIS] = labs(da - dc);
#elif ENABLED(COREYZ) #elif ENABLED(COREYZ)
// coreyz planning // coreyz planning
block->steps[X_AXIS] = labs(dx); block->steps[X_AXIS] = labs(da);
block->steps[B_AXIS] = labs(dy + dz); block->steps[B_AXIS] = labs(db + dc);
block->steps[C_AXIS] = labs(dy - dz); block->steps[C_AXIS] = labs(db - dc);
#else #else
// default non-h-bot planning // default non-h-bot planning
block->steps[X_AXIS] = labs(dx); block->steps[X_AXIS] = labs(da);
block->steps[Y_AXIS] = labs(dy); block->steps[Y_AXIS] = labs(db);
block->steps[Z_AXIS] = labs(dz); block->steps[Z_AXIS] = labs(dc);
#endif #endif
block->steps[E_AXIS] = labs(de) * volumetric_multiplier[extruder] * flow_percentage[extruder] * 0.01 + 0.5; block->steps[E_AXIS] = labs(de) * volumetric_multiplier[extruder] * flow_percentage[extruder] * 0.01 + 0.5;
@ -720,33 +721,33 @@ void Planner::_buffer_line(const float &lx, const float &ly, const float &lz, co
block->e_to_p_pressure = baricuda_e_to_p_pressure; block->e_to_p_pressure = baricuda_e_to_p_pressure;
#endif #endif
// Compute direction bits for this block // Compute direction bit-mask for this block
uint8_t db = 0; uint8_t dm = 0;
#if ENABLED(COREXY) #if ENABLED(COREXY)
if (dx < 0) SBI(db, X_HEAD); // Save the real Extruder (head) direction in X Axis if (da < 0) SBI(dm, X_HEAD); // Save the real Extruder (head) direction in X Axis
if (dy < 0) SBI(db, Y_HEAD); // ...and Y if (db < 0) SBI(dm, Y_HEAD); // ...and Y
if (dz < 0) SBI(db, Z_AXIS); if (dc < 0) SBI(dm, Z_AXIS);
if (dx + dy < 0) SBI(db, A_AXIS); // Motor A direction if (da + db < 0) SBI(dm, A_AXIS); // Motor A direction
if (dx - dy < 0) SBI(db, B_AXIS); // Motor B direction if (da - db < 0) SBI(dm, B_AXIS); // Motor B direction
#elif ENABLED(COREXZ) #elif ENABLED(COREXZ)
if (dx < 0) SBI(db, X_HEAD); // Save the real Extruder (head) direction in X Axis if (da < 0) SBI(dm, X_HEAD); // Save the real Extruder (head) direction in X Axis
if (dy < 0) SBI(db, Y_AXIS); if (db < 0) SBI(dm, Y_AXIS);
if (dz < 0) SBI(db, Z_HEAD); // ...and Z if (dc < 0) SBI(dm, Z_HEAD); // ...and Z
if (dx + dz < 0) SBI(db, A_AXIS); // Motor A direction if (da + dc < 0) SBI(dm, A_AXIS); // Motor A direction
if (dx - dz < 0) SBI(db, C_AXIS); // Motor C direction if (da - dc < 0) SBI(dm, C_AXIS); // Motor C direction
#elif ENABLED(COREYZ) #elif ENABLED(COREYZ)
if (dx < 0) SBI(db, X_AXIS); if (da < 0) SBI(dm, X_AXIS);
if (dy < 0) SBI(db, Y_HEAD); // Save the real Extruder (head) direction in Y Axis if (db < 0) SBI(dm, Y_HEAD); // Save the real Extruder (head) direction in Y Axis
if (dz < 0) SBI(db, Z_HEAD); // ...and Z if (dc < 0) SBI(dm, Z_HEAD); // ...and Z
if (dy + dz < 0) SBI(db, B_AXIS); // Motor B direction if (db + dc < 0) SBI(dm, B_AXIS); // Motor B direction
if (dy - dz < 0) SBI(db, C_AXIS); // Motor C direction if (db - dc < 0) SBI(dm, C_AXIS); // Motor C direction
#else #else
if (dx < 0) SBI(db, X_AXIS); if (da < 0) SBI(dm, X_AXIS);
if (dy < 0) SBI(db, Y_AXIS); if (db < 0) SBI(dm, Y_AXIS);
if (dz < 0) SBI(db, Z_AXIS); if (dc < 0) SBI(dm, Z_AXIS);
#endif #endif
if (de < 0) SBI(db, E_AXIS); if (de < 0) SBI(dm, E_AXIS);
block->direction_bits = db; block->direction_bits = dm;
block->active_extruder = extruder; block->active_extruder = extruder;
@ -859,29 +860,29 @@ void Planner::_buffer_line(const float &lx, const float &ly, const float &lz, co
#if ENABLED(COREXY) || ENABLED(COREXZ) || ENABLED(COREYZ) #if ENABLED(COREXY) || ENABLED(COREXZ) || ENABLED(COREYZ)
float delta_mm[7]; float delta_mm[7];
#if ENABLED(COREXY) #if ENABLED(COREXY)
delta_mm[X_HEAD] = dx * steps_to_mm[A_AXIS]; delta_mm[X_HEAD] = da * steps_to_mm[A_AXIS];
delta_mm[Y_HEAD] = dy * steps_to_mm[B_AXIS]; delta_mm[Y_HEAD] = db * steps_to_mm[B_AXIS];
delta_mm[Z_AXIS] = dz * steps_to_mm[Z_AXIS]; delta_mm[Z_AXIS] = dc * steps_to_mm[Z_AXIS];
delta_mm[A_AXIS] = (dx + dy) * steps_to_mm[A_AXIS]; delta_mm[A_AXIS] = (da + db) * steps_to_mm[A_AXIS];
delta_mm[B_AXIS] = (dx - dy) * steps_to_mm[B_AXIS]; delta_mm[B_AXIS] = (da - db) * steps_to_mm[B_AXIS];
#elif ENABLED(COREXZ) #elif ENABLED(COREXZ)
delta_mm[X_HEAD] = dx * steps_to_mm[A_AXIS]; delta_mm[X_HEAD] = da * steps_to_mm[A_AXIS];
delta_mm[Y_AXIS] = dy * steps_to_mm[Y_AXIS]; delta_mm[Y_AXIS] = db * steps_to_mm[Y_AXIS];
delta_mm[Z_HEAD] = dz * steps_to_mm[C_AXIS]; delta_mm[Z_HEAD] = dc * steps_to_mm[C_AXIS];
delta_mm[A_AXIS] = (dx + dz) * steps_to_mm[A_AXIS]; delta_mm[A_AXIS] = (da + dc) * steps_to_mm[A_AXIS];
delta_mm[C_AXIS] = (dx - dz) * steps_to_mm[C_AXIS]; delta_mm[C_AXIS] = (da - dc) * steps_to_mm[C_AXIS];
#elif ENABLED(COREYZ) #elif ENABLED(COREYZ)
delta_mm[X_AXIS] = dx * steps_to_mm[X_AXIS]; delta_mm[X_AXIS] = da * steps_to_mm[X_AXIS];
delta_mm[Y_HEAD] = dy * steps_to_mm[B_AXIS]; delta_mm[Y_HEAD] = db * steps_to_mm[B_AXIS];
delta_mm[Z_HEAD] = dz * steps_to_mm[C_AXIS]; delta_mm[Z_HEAD] = dc * steps_to_mm[C_AXIS];
delta_mm[B_AXIS] = (dy + dz) * steps_to_mm[B_AXIS]; delta_mm[B_AXIS] = (db + dc) * steps_to_mm[B_AXIS];
delta_mm[C_AXIS] = (dy - dz) * steps_to_mm[C_AXIS]; delta_mm[C_AXIS] = (db - dc) * steps_to_mm[C_AXIS];
#endif #endif
#else #else
float delta_mm[4]; float delta_mm[4];
delta_mm[X_AXIS] = dx * steps_to_mm[X_AXIS]; delta_mm[X_AXIS] = da * steps_to_mm[X_AXIS];
delta_mm[Y_AXIS] = dy * steps_to_mm[Y_AXIS]; delta_mm[Y_AXIS] = db * steps_to_mm[Y_AXIS];
delta_mm[Z_AXIS] = dz * steps_to_mm[Z_AXIS]; delta_mm[Z_AXIS] = dc * steps_to_mm[Z_AXIS];
#endif #endif
delta_mm[E_AXIS] = 0.01 * (de * steps_to_mm[E_AXIS]) * volumetric_multiplier[extruder] * flow_percentage[extruder]; delta_mm[E_AXIS] = 0.01 * (de * steps_to_mm[E_AXIS]) * volumetric_multiplier[extruder] * flow_percentage[extruder];
@ -1184,12 +1185,12 @@ void Planner::_buffer_line(const float &lx, const float &ly, const float &lz, co
* On CORE machines stepper ABC will be translated from the given XYZ. * On CORE machines stepper ABC will be translated from the given XYZ.
*/ */
void Planner::_set_position_mm(const float &lx, const float &ly, const float &lz, const float &e) { void Planner::_set_position_mm(const float &a, const float &b, const float &c, const float &e) {
long nx = position[X_AXIS] = lround(lx * axis_steps_per_mm[X_AXIS]), long na = position[X_AXIS] = lround(a * axis_steps_per_mm[X_AXIS]),
ny = position[Y_AXIS] = lround(ly * axis_steps_per_mm[Y_AXIS]), nb = position[Y_AXIS] = lround(b * axis_steps_per_mm[Y_AXIS]),
nz = position[Z_AXIS] = lround(lz * axis_steps_per_mm[Z_AXIS]), nc = position[Z_AXIS] = lround(c * axis_steps_per_mm[Z_AXIS]),
ne = position[E_AXIS] = lround(e * axis_steps_per_mm[E_AXIS]); ne = position[E_AXIS] = lround(e * axis_steps_per_mm[E_AXIS]);
stepper.set_position(nx, ny, nz, ne); 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.
memset(previous_speed, 0, sizeof(previous_speed)); memset(previous_speed, 0, sizeof(previous_speed));

@ -232,27 +232,32 @@ class Planner {
/** /**
* Planner::_buffer_line * Planner::_buffer_line
* *
* Add a new linear movement to the buffer. * Add a new direct linear movement to the buffer.
* Doesn't apply the leveling. *
* Leveling and kinematics should be applied ahead of this.
* *
* x,y,z,e - target position in mm * a,b,c,e - target position in mm or degrees
* fr_mm_s - (target) speed of the move * fr_mm_s - (target) speed of the move (mm/s)
* extruder - target extruder * extruder - target extruder
*/ */
static void _buffer_line(const float &lx, const float &ly, const float &lz, const float &e, float fr_mm_s, const uint8_t extruder); static void _buffer_line(const float &a, const float &b, const float &c, const float &e, float fr_mm_s, const uint8_t extruder);
static void _set_position_mm(const float &lx, const float &ly, const float &lz, const float &e); static void _set_position_mm(const float &a, const float &b, const float &c, const float &e);
/** /**
* Add a new linear movement to the buffer. * Add a new linear movement to the buffer.
* The target is NOT translated to delta/scara * The target is NOT translated to delta/scara
* *
* x,y,z,e - target position in mm * Leveling will be applied to input on cartesians.
* fr_mm_s - (target) speed of the move (mm/s) * Kinematic machines should call buffer_line_kinematic (for leveled moves).
* extruder - target extruder * (Cartesians may also call buffer_line_kinematic.)
*
* lx,ly,lz,e - target position in mm or degrees
* fr_mm_s - (target) speed of the move (mm/s)
* extruder - target extruder
*/ */
static FORCE_INLINE void buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, float fr_mm_s, const uint8_t extruder) { static FORCE_INLINE void buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, float fr_mm_s, const uint8_t extruder) {
#if PLANNER_LEVELING && ! IS_KINEMATIC #if PLANNER_LEVELING && IS_CARTESIAN
apply_leveling(lx, ly, lz); apply_leveling(lx, ly, lz);
#endif #endif
_buffer_line(lx, ly, lz, e, fr_mm_s, extruder); _buffer_line(lx, ly, lz, e, fr_mm_s, extruder);
@ -267,7 +272,7 @@ class Planner {
* fr_mm_s - (target) speed of the move (mm/s) * fr_mm_s - (target) speed of the move (mm/s)
* extruder - target extruder * extruder - target extruder
*/ */
static FORCE_INLINE void buffer_line_kinematic(const float target[NUM_AXIS], float fr_mm_s, const uint8_t extruder) { static FORCE_INLINE void buffer_line_kinematic(const float target[XYZE], float fr_mm_s, const uint8_t extruder) {
#if PLANNER_LEVELING #if PLANNER_LEVELING
float pos[XYZ] = { target[X_AXIS], target[Y_AXIS], target[Z_AXIS] }; float pos[XYZ] = { target[X_AXIS], target[Y_AXIS], target[Z_AXIS] };
apply_leveling(pos); apply_leveling(pos);
@ -292,7 +297,7 @@ class Planner {
* Clears previous speed values. * Clears previous speed values.
*/ */
static FORCE_INLINE void set_position_mm(ARG_X, ARG_Y, ARG_Z, const float &e) { static FORCE_INLINE void set_position_mm(ARG_X, ARG_Y, ARG_Z, const float &e) {
#if PLANNER_LEVELING && ! IS_KINEMATIC #if PLANNER_LEVELING && IS_CARTESIAN
apply_leveling(lx, ly, lz); apply_leveling(lx, ly, lz);
#endif #endif
_set_position_mm(lx, ly, lz, e); _set_position_mm(lx, ly, lz, e);

@ -953,7 +953,7 @@ void Stepper::synchronize() { while (planner.blocks_queued()) idle(); }
* 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& x, const long& y, const long& z, const long& e) { void Stepper::set_position(const long &a, const long &b, const long &c, const long &e) {
synchronize(); // Bad to set stepper counts in the middle of a move synchronize(); // Bad to set stepper counts in the middle of a move
@ -962,37 +962,37 @@ void Stepper::set_position(const long& x, const long& y, const long& z, const lo
#if ENABLED(COREXY) #if ENABLED(COREXY)
// 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
count_position[A_AXIS] = x + y; count_position[A_AXIS] = a + b;
count_position[B_AXIS] = x - y; count_position[B_AXIS] = a - b;
count_position[Z_AXIS] = z; count_position[Z_AXIS] = c;
#elif ENABLED(COREXZ) #elif ENABLED(COREXZ)
// corexz planning // corexz planning
count_position[A_AXIS] = x + z; count_position[A_AXIS] = a + c;
count_position[Y_AXIS] = y; count_position[Y_AXIS] = b;
count_position[C_AXIS] = x - z; count_position[C_AXIS] = a - c;
#elif ENABLED(COREYZ) #elif ENABLED(COREYZ)
// coreyz planning // coreyz planning
count_position[X_AXIS] = x; count_position[X_AXIS] = a;
count_position[B_AXIS] = y + z; count_position[B_AXIS] = y + c;
count_position[C_AXIS] = y - z; count_position[C_AXIS] = y - c;
#else #else
// default non-h-bot planning // default non-h-bot planning
count_position[X_AXIS] = x; count_position[X_AXIS] = a;
count_position[Y_AXIS] = y; count_position[Y_AXIS] = b;
count_position[Z_AXIS] = z; count_position[Z_AXIS] = c;
#endif #endif
count_position[E_AXIS] = e; count_position[E_AXIS] = e;
CRITICAL_SECTION_END; CRITICAL_SECTION_END;
} }
void Stepper::set_position(const AxisEnum &axis, const long& v) { void Stepper::set_position(const AxisEnum &axis, const long &v) {
CRITICAL_SECTION_START; CRITICAL_SECTION_START;
count_position[axis] = v; count_position[axis] = v;
CRITICAL_SECTION_END; CRITICAL_SECTION_END;
} }
void Stepper::set_e_position(const long& e) { void Stepper::set_e_position(const long &e) {
CRITICAL_SECTION_START; CRITICAL_SECTION_START;
count_position[E_AXIS] = e; count_position[E_AXIS] = e;
CRITICAL_SECTION_END; CRITICAL_SECTION_END;

@ -188,9 +188,9 @@ class Stepper {
// //
// Set the current position in steps // Set the current position in steps
// //
static void set_position(const long& x, const long& y, const long& z, const long& e); static void set_position(const long &a, const long &b, const long &c, const long &e);
static void set_position(const AxisEnum& a, const long& v); static void set_position(const AxisEnum &a, const long &v);
static void set_e_position(const long& e); static void set_e_position(const long &e);
// //
// Set direction bits for all steppers // Set direction bits for all steppers

Loading…
Cancel
Save