@@ -1614,6 +1614,7 @@ void Planner::finish_and_disable() {
16141614float Planner::get_axis_position_mm (const AxisEnum axis) {
16151615 float axis_steps;
16161616 #if IS_CORE
1617+
16171618 // Requesting one of the "core" axes?
16181619 if (axis == CORE_AXIS_1 || axis == CORE_AXIS_2) {
16191620
@@ -1631,9 +1632,30 @@ float Planner::get_axis_position_mm(const AxisEnum axis) {
16311632 }
16321633 else
16331634 axis_steps = stepper.position (axis);
1635+
1636+ #elif ENABLED(MARKFORGED_XY)
1637+
1638+ // Requesting one of the joined axes?
1639+ if (axis == CORE_AXIS_1 || axis == CORE_AXIS_2) {
1640+ // Protect the access to the position.
1641+ const bool was_enabled = stepper.suspend ();
1642+
1643+ const int32_t p1 = stepper.position (CORE_AXIS_1),
1644+ p2 = stepper.position (CORE_AXIS_2);
1645+
1646+ if (was_enabled) stepper.wake_up ();
1647+
1648+ axis_steps = ((axis == CORE_AXIS_1) ? p1 - p2 : p2);
1649+ }
1650+ else
1651+ axis_steps = stepper.position (axis);
1652+
16341653 #else
1654+
16351655 axis_steps = stepper.position (axis);
1656+
16361657 #endif
1658+
16371659 return axis_steps * steps_to_mm[axis];
16381660}
16391661
@@ -1808,6 +1830,12 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
18081830 if (dc < 0 ) SBI (dm, Z_HEAD); // ...and Z
18091831 if (db + dc < 0 ) SBI (dm, B_AXIS); // Motor B direction
18101832 if (CORESIGN (db - dc) < 0 ) SBI (dm, C_AXIS); // Motor C direction
1833+ #elif ENABLED(MARKFORGED_XY)
1834+ if (da < 0 ) SBI (dm, X_HEAD); // Save the real Extruder (head) direction in X Axis
1835+ if (db < 0 ) SBI (dm, Y_HEAD); // ...and Y
1836+ if (dc < 0 ) SBI (dm, Z_AXIS);
1837+ if (da + db < 0 ) SBI (dm, A_AXIS); // Motor A direction
1838+ if (db < 0 ) SBI (dm, B_AXIS); // Motor B direction
18111839 #else
18121840 if (da < 0 ) SBI (dm, X_AXIS);
18131841 if (db < 0 ) SBI (dm, Y_AXIS);
@@ -1843,6 +1871,8 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
18431871 block->steps .set (ABS (da + dc), ABS (db), ABS (da - dc));
18441872 #elif CORE_IS_YZ
18451873 block->steps .set (ABS (da), ABS (db + dc), ABS (db - dc));
1874+ #elif ENABLED(MARKFORGED_XY)
1875+ block->steps .set (ABS (da + db), ABS (db), ABS (dc));
18461876 #elif IS_SCARA
18471877 block->steps .set (ABS (da), ABS (db), ABS (dc));
18481878 #else
@@ -1859,7 +1889,9 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
18591889 * Having the real displacement of the head, we can calculate the total movement length and apply the desired speed.
18601890 */
18611891 struct DistanceMM : abce_float_t {
1862- TERN_ (IS_CORE, xyz_pos_t head);
1892+ #if EITHER(IS_CORE, MARKFORGED_XY)
1893+ xyz_pos_t head;
1894+ #endif
18631895 } steps_dist_mm;
18641896 #if IS_CORE
18651897 #if CORE_IS_XY
@@ -1881,6 +1913,12 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
18811913 steps_dist_mm.b = (db + dc) * steps_to_mm[B_AXIS];
18821914 steps_dist_mm.c = CORESIGN (db - dc) * steps_to_mm[C_AXIS];
18831915 #endif
1916+ #elif ENABLED(MARKFORGED_XY)
1917+ steps_dist_mm.head .x = da * steps_to_mm[A_AXIS];
1918+ steps_dist_mm.head .y = db * steps_to_mm[B_AXIS];
1919+ steps_dist_mm.z = dc * steps_to_mm[Z_AXIS];
1920+ steps_dist_mm.a = (da - db) * steps_to_mm[A_AXIS];
1921+ steps_dist_mm.b = db * steps_to_mm[B_AXIS];
18841922 #else
18851923 steps_dist_mm.a = da * steps_to_mm[A_AXIS];
18861924 steps_dist_mm.b = db * steps_to_mm[B_AXIS];
@@ -1907,7 +1945,7 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
19071945 block->millimeters = millimeters;
19081946 else
19091947 block->millimeters = SQRT (
1910- #if CORE_IS_XY
1948+ #if EITHER( CORE_IS_XY, MARKFORGED_XY)
19111949 sq (steps_dist_mm.head .x ) + sq (steps_dist_mm.head .y ) + sq (steps_dist_mm.z )
19121950 #elif CORE_IS_XZ
19131951 sq (steps_dist_mm.head .x ) + sq (steps_dist_mm.y ) + sq (steps_dist_mm.head .z )
@@ -1964,7 +2002,7 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
19642002 #endif
19652003
19662004 // Enable active axes
1967- #if CORE_IS_XY
2005+ #if EITHER( CORE_IS_XY, MARKFORGED_XY)
19682006 if (block->steps .a || block->steps .b ) {
19692007 ENABLE_AXIS_X ();
19702008 ENABLE_AXIS_Y ();
@@ -2325,9 +2363,9 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
23252363 * On CoreXY the length of the vector [A,B] is SQRT(2) times the length of the head movement vector [X,Y].
23262364 * So taking Z and E into account, we cannot scale to a unit vector with "inverse_millimeters".
23272365 * => normalize the complete junction vector.
2328- * Elsewise, when needed JD factors in the E component
2366+ * Elsewise, when needed JD will factor- in the E component
23292367 */
2330- if (ENABLED (IS_CORE) || esteps > 0 )
2368+ if (EITHER (IS_CORE, MARKFORGED_XY ) || esteps > 0 )
23312369 normalize_junction_vector (unit_vec); // Normalize with XYZE components
23322370 else
23332371 unit_vec *= inverse_millimeters; // Use pre-calculated (1 / SQRT(x^2 + y^2 + z^2))
0 commit comments