|
|
|
|
@ -73,7 +73,7 @@ bool relative_mode = false;
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
* Cartesian Current Position
|
|
|
|
|
* Used to track the logical position as moves are queued.
|
|
|
|
|
* Used to track the native machine position as moves are queued.
|
|
|
|
|
* Used by 'line_to_current_position' to do a move after changing it.
|
|
|
|
|
* Used by 'SYNC_PLAN_POSITION_KINEMATIC' to update 'planner.position'.
|
|
|
|
|
*/
|
|
|
|
|
@ -197,20 +197,16 @@ void get_cartesian_from_steppers() {
|
|
|
|
|
stepper.get_axis_position_mm(B_AXIS),
|
|
|
|
|
stepper.get_axis_position_mm(C_AXIS)
|
|
|
|
|
);
|
|
|
|
|
cartes[X_AXIS] += LOGICAL_X_POSITION(0);
|
|
|
|
|
cartes[Y_AXIS] += LOGICAL_Y_POSITION(0);
|
|
|
|
|
cartes[Z_AXIS] += LOGICAL_Z_POSITION(0);
|
|
|
|
|
#elif IS_SCARA
|
|
|
|
|
#else
|
|
|
|
|
#if IS_SCARA
|
|
|
|
|
forward_kinematics_SCARA(
|
|
|
|
|
stepper.get_axis_position_degrees(A_AXIS),
|
|
|
|
|
stepper.get_axis_position_degrees(B_AXIS)
|
|
|
|
|
);
|
|
|
|
|
cartes[X_AXIS] += LOGICAL_X_POSITION(0);
|
|
|
|
|
cartes[Y_AXIS] += LOGICAL_Y_POSITION(0);
|
|
|
|
|
cartes[Z_AXIS] = stepper.get_axis_position_mm(Z_AXIS);
|
|
|
|
|
#else
|
|
|
|
|
cartes[X_AXIS] = stepper.get_axis_position_mm(X_AXIS);
|
|
|
|
|
cartes[Y_AXIS] = stepper.get_axis_position_mm(Y_AXIS);
|
|
|
|
|
#endif
|
|
|
|
|
cartes[Z_AXIS] = stepper.get_axis_position_mm(Z_AXIS);
|
|
|
|
|
#endif
|
|
|
|
|
}
|
|
|
|
|
@ -288,16 +284,16 @@ void line_to_destination(const float fr_mm_s) {
|
|
|
|
|
* Plan a move to (X, Y, Z) and set the current_position
|
|
|
|
|
* The final current_position may not be the one that was requested
|
|
|
|
|
*/
|
|
|
|
|
void do_blocking_move_to(const float &lx, const float &ly, const float &lz, const float &fr_mm_s/*=0.0*/) {
|
|
|
|
|
void do_blocking_move_to(const float &rx, const float &ry, const float &rz, const float &fr_mm_s/*=0.0*/) {
|
|
|
|
|
const float old_feedrate_mm_s = feedrate_mm_s;
|
|
|
|
|
|
|
|
|
|
#if ENABLED(DEBUG_LEVELING_FEATURE)
|
|
|
|
|
if (DEBUGGING(LEVELING)) print_xyz(PSTR(">>> do_blocking_move_to"), NULL, lx, ly, lz);
|
|
|
|
|
if (DEBUGGING(LEVELING)) print_xyz(PSTR(">>> do_blocking_move_to"), NULL, rx, ry, rz);
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
#if ENABLED(DELTA)
|
|
|
|
|
|
|
|
|
|
if (!position_is_reachable_xy(lx, ly)) return;
|
|
|
|
|
if (!position_is_reachable(rx, ry)) return;
|
|
|
|
|
|
|
|
|
|
feedrate_mm_s = fr_mm_s ? fr_mm_s : XY_PROBE_FEEDRATE_MM_S;
|
|
|
|
|
|
|
|
|
|
@ -309,10 +305,10 @@ void do_blocking_move_to(const float &lx, const float &ly, const float &lz, cons
|
|
|
|
|
|
|
|
|
|
// when in the danger zone
|
|
|
|
|
if (current_position[Z_AXIS] > delta_clip_start_height) {
|
|
|
|
|
if (lz > delta_clip_start_height) { // staying in the danger zone
|
|
|
|
|
destination[X_AXIS] = lx; // move directly (uninterpolated)
|
|
|
|
|
destination[Y_AXIS] = ly;
|
|
|
|
|
destination[Z_AXIS] = lz;
|
|
|
|
|
if (rz > delta_clip_start_height) { // staying in the danger zone
|
|
|
|
|
destination[X_AXIS] = rx; // move directly (uninterpolated)
|
|
|
|
|
destination[Y_AXIS] = ry;
|
|
|
|
|
destination[Z_AXIS] = rz;
|
|
|
|
|
prepare_uninterpolated_move_to_destination(); // set_current_from_destination()
|
|
|
|
|
#if ENABLED(DEBUG_LEVELING_FEATURE)
|
|
|
|
|
if (DEBUGGING(LEVELING)) DEBUG_POS("danger zone move", current_position);
|
|
|
|
|
@ -328,23 +324,23 @@ void do_blocking_move_to(const float &lx, const float &ly, const float &lz, cons
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if (lz > current_position[Z_AXIS]) { // raising?
|
|
|
|
|
destination[Z_AXIS] = lz;
|
|
|
|
|
if (rz > current_position[Z_AXIS]) { // raising?
|
|
|
|
|
destination[Z_AXIS] = rz;
|
|
|
|
|
prepare_uninterpolated_move_to_destination(); // set_current_from_destination()
|
|
|
|
|
#if ENABLED(DEBUG_LEVELING_FEATURE)
|
|
|
|
|
if (DEBUGGING(LEVELING)) DEBUG_POS("z raise move", current_position);
|
|
|
|
|
#endif
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
destination[X_AXIS] = lx;
|
|
|
|
|
destination[Y_AXIS] = ly;
|
|
|
|
|
destination[X_AXIS] = rx;
|
|
|
|
|
destination[Y_AXIS] = ry;
|
|
|
|
|
prepare_move_to_destination(); // set_current_from_destination()
|
|
|
|
|
#if ENABLED(DEBUG_LEVELING_FEATURE)
|
|
|
|
|
if (DEBUGGING(LEVELING)) DEBUG_POS("xy move", current_position);
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
if (lz < current_position[Z_AXIS]) { // lowering?
|
|
|
|
|
destination[Z_AXIS] = lz;
|
|
|
|
|
if (rz < current_position[Z_AXIS]) { // lowering?
|
|
|
|
|
destination[Z_AXIS] = rz;
|
|
|
|
|
prepare_uninterpolated_move_to_destination(); // set_current_from_destination()
|
|
|
|
|
#if ENABLED(DEBUG_LEVELING_FEATURE)
|
|
|
|
|
if (DEBUGGING(LEVELING)) DEBUG_POS("z lower move", current_position);
|
|
|
|
|
@ -353,44 +349,44 @@ void do_blocking_move_to(const float &lx, const float &ly, const float &lz, cons
|
|
|
|
|
|
|
|
|
|
#elif IS_SCARA
|
|
|
|
|
|
|
|
|
|
if (!position_is_reachable_xy(lx, ly)) return;
|
|
|
|
|
if (!position_is_reachable(rx, ry)) return;
|
|
|
|
|
|
|
|
|
|
set_destination_from_current();
|
|
|
|
|
|
|
|
|
|
// If Z needs to raise, do it before moving XY
|
|
|
|
|
if (destination[Z_AXIS] < lz) {
|
|
|
|
|
destination[Z_AXIS] = lz;
|
|
|
|
|
if (destination[Z_AXIS] < rz) {
|
|
|
|
|
destination[Z_AXIS] = rz;
|
|
|
|
|
prepare_uninterpolated_move_to_destination(fr_mm_s ? fr_mm_s : homing_feedrate(Z_AXIS));
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
destination[X_AXIS] = lx;
|
|
|
|
|
destination[Y_AXIS] = ly;
|
|
|
|
|
destination[X_AXIS] = rx;
|
|
|
|
|
destination[Y_AXIS] = ry;
|
|
|
|
|
prepare_uninterpolated_move_to_destination(fr_mm_s ? fr_mm_s : XY_PROBE_FEEDRATE_MM_S);
|
|
|
|
|
|
|
|
|
|
// If Z needs to lower, do it after moving XY
|
|
|
|
|
if (destination[Z_AXIS] > lz) {
|
|
|
|
|
destination[Z_AXIS] = lz;
|
|
|
|
|
if (destination[Z_AXIS] > rz) {
|
|
|
|
|
destination[Z_AXIS] = rz;
|
|
|
|
|
prepare_uninterpolated_move_to_destination(fr_mm_s ? fr_mm_s : homing_feedrate(Z_AXIS));
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
#else
|
|
|
|
|
|
|
|
|
|
// If Z needs to raise, do it before moving XY
|
|
|
|
|
if (current_position[Z_AXIS] < lz) {
|
|
|
|
|
if (current_position[Z_AXIS] < rz) {
|
|
|
|
|
feedrate_mm_s = fr_mm_s ? fr_mm_s : homing_feedrate(Z_AXIS);
|
|
|
|
|
current_position[Z_AXIS] = lz;
|
|
|
|
|
current_position[Z_AXIS] = rz;
|
|
|
|
|
line_to_current_position();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
feedrate_mm_s = fr_mm_s ? fr_mm_s : XY_PROBE_FEEDRATE_MM_S;
|
|
|
|
|
current_position[X_AXIS] = lx;
|
|
|
|
|
current_position[Y_AXIS] = ly;
|
|
|
|
|
current_position[X_AXIS] = rx;
|
|
|
|
|
current_position[Y_AXIS] = ry;
|
|
|
|
|
line_to_current_position();
|
|
|
|
|
|
|
|
|
|
// If Z needs to lower, do it after moving XY
|
|
|
|
|
if (current_position[Z_AXIS] > lz) {
|
|
|
|
|
if (current_position[Z_AXIS] > rz) {
|
|
|
|
|
feedrate_mm_s = fr_mm_s ? fr_mm_s : homing_feedrate(Z_AXIS);
|
|
|
|
|
current_position[Z_AXIS] = lz;
|
|
|
|
|
current_position[Z_AXIS] = rz;
|
|
|
|
|
line_to_current_position();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
@ -404,14 +400,14 @@ void do_blocking_move_to(const float &lx, const float &ly, const float &lz, cons
|
|
|
|
|
if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("<<< do_blocking_move_to");
|
|
|
|
|
#endif
|
|
|
|
|
}
|
|
|
|
|
void do_blocking_move_to_x(const float &lx, const float &fr_mm_s/*=0.0*/) {
|
|
|
|
|
do_blocking_move_to(lx, current_position[Y_AXIS], current_position[Z_AXIS], fr_mm_s);
|
|
|
|
|
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);
|
|
|
|
|
}
|
|
|
|
|
void do_blocking_move_to_z(const float &lz, const float &fr_mm_s/*=0.0*/) {
|
|
|
|
|
do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], lz, fr_mm_s);
|
|
|
|
|
void do_blocking_move_to_z(const float &rz, const float &fr_mm_s/*=0.0*/) {
|
|
|
|
|
do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], rz, fr_mm_s);
|
|
|
|
|
}
|
|
|
|
|
void do_blocking_move_to_xy(const float &lx, const float &ly, const float &fr_mm_s/*=0.0*/) {
|
|
|
|
|
do_blocking_move_to(lx, ly, current_position[Z_AXIS], fr_mm_s);
|
|
|
|
|
void do_blocking_move_to_xy(const float &rx, const float &ry, const float &fr_mm_s/*=0.0*/) {
|
|
|
|
|
do_blocking_move_to(rx, ry, current_position[Z_AXIS], fr_mm_s);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
//
|
|
|
|
|
@ -521,26 +517,26 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
|
|
|
|
|
* This calls planner.buffer_line several times, adding
|
|
|
|
|
* small incremental moves for DELTA or SCARA.
|
|
|
|
|
*/
|
|
|
|
|
inline bool prepare_kinematic_move_to(float ltarget[XYZE]) {
|
|
|
|
|
inline bool prepare_kinematic_move_to(float rtarget[XYZE]) {
|
|
|
|
|
|
|
|
|
|
// Get the top feedrate of the move in the XY plane
|
|
|
|
|
const float _feedrate_mm_s = MMS_SCALED(feedrate_mm_s);
|
|
|
|
|
|
|
|
|
|
// If the move is only in Z/E don't split up the move
|
|
|
|
|
if (ltarget[X_AXIS] == current_position[X_AXIS] && ltarget[Y_AXIS] == current_position[Y_AXIS]) {
|
|
|
|
|
planner.buffer_line_kinematic(ltarget, _feedrate_mm_s, active_extruder);
|
|
|
|
|
if (rtarget[X_AXIS] == current_position[X_AXIS] && rtarget[Y_AXIS] == current_position[Y_AXIS]) {
|
|
|
|
|
planner.buffer_line_kinematic(rtarget, _feedrate_mm_s, active_extruder);
|
|
|
|
|
return false;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Fail if attempting move outside printable radius
|
|
|
|
|
if (!position_is_reachable_xy(ltarget[X_AXIS], ltarget[Y_AXIS])) return true;
|
|
|
|
|
if (!position_is_reachable(rtarget[X_AXIS], rtarget[Y_AXIS])) return true;
|
|
|
|
|
|
|
|
|
|
// Get the cartesian distances moved in XYZE
|
|
|
|
|
const float difference[XYZE] = {
|
|
|
|
|
ltarget[X_AXIS] - current_position[X_AXIS],
|
|
|
|
|
ltarget[Y_AXIS] - current_position[Y_AXIS],
|
|
|
|
|
ltarget[Z_AXIS] - current_position[Z_AXIS],
|
|
|
|
|
ltarget[E_AXIS] - current_position[E_AXIS]
|
|
|
|
|
rtarget[X_AXIS] - current_position[X_AXIS],
|
|
|
|
|
rtarget[Y_AXIS] - current_position[Y_AXIS],
|
|
|
|
|
rtarget[Z_AXIS] - current_position[Z_AXIS],
|
|
|
|
|
rtarget[E_AXIS] - current_position[E_AXIS]
|
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
// Get the linear distance in XYZ
|
|
|
|
|
@ -588,9 +584,9 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
|
|
|
|
|
oldB = stepper.get_axis_position_degrees(B_AXIS);
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
// Get the logical current position as starting point
|
|
|
|
|
float logical[XYZE];
|
|
|
|
|
COPY(logical, current_position);
|
|
|
|
|
// Get the current position as starting point
|
|
|
|
|
float raw[XYZE];
|
|
|
|
|
COPY(raw, current_position);
|
|
|
|
|
|
|
|
|
|
// Drop one segment so the last move is to the exact target.
|
|
|
|
|
// If there's only 1 segment, loops will be skipped entirely.
|
|
|
|
|
@ -598,25 +594,25 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
|
|
|
|
|
|
|
|
|
|
// Calculate and execute the segments
|
|
|
|
|
for (uint16_t s = segments + 1; --s;) {
|
|
|
|
|
LOOP_XYZE(i) logical[i] += segment_distance[i];
|
|
|
|
|
LOOP_XYZE(i) raw[i] += segment_distance[i];
|
|
|
|
|
#if ENABLED(DELTA)
|
|
|
|
|
DELTA_LOGICAL_IK(); // Delta can inline its kinematics
|
|
|
|
|
DELTA_RAW_IK(); // Delta can inline its kinematics
|
|
|
|
|
#else
|
|
|
|
|
inverse_kinematics(logical);
|
|
|
|
|
inverse_kinematics(raw);
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
ADJUST_DELTA(logical); // Adjust Z if bed leveling is enabled
|
|
|
|
|
ADJUST_DELTA(raw); // Adjust Z if bed leveling is enabled
|
|
|
|
|
|
|
|
|
|
#if IS_SCARA && ENABLED(SCARA_FEEDRATE_SCALING)
|
|
|
|
|
// For SCARA scale the feed rate from mm/s to degrees/s
|
|
|
|
|
// Use ratio between the length of the move and the larger angle change
|
|
|
|
|
const float adiff = abs(delta[A_AXIS] - oldA),
|
|
|
|
|
bdiff = abs(delta[B_AXIS] - oldB);
|
|
|
|
|
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], logical[E_AXIS], max(adiff, bdiff) * feed_factor, active_extruder);
|
|
|
|
|
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], max(adiff, bdiff) * feed_factor, active_extruder);
|
|
|
|
|
oldA = delta[A_AXIS];
|
|
|
|
|
oldB = delta[B_AXIS];
|
|
|
|
|
#else
|
|
|
|
|
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], logical[E_AXIS], _feedrate_mm_s, active_extruder);
|
|
|
|
|
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], _feedrate_mm_s, active_extruder);
|
|
|
|
|
#endif
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
@ -626,13 +622,13 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
|
|
|
|
|
#if IS_SCARA && ENABLED(SCARA_FEEDRATE_SCALING)
|
|
|
|
|
// For SCARA scale the feed rate from mm/s to degrees/s
|
|
|
|
|
// With segments > 1 length is 1 segment, otherwise total length
|
|
|
|
|
inverse_kinematics(ltarget);
|
|
|
|
|
ADJUST_DELTA(ltarget);
|
|
|
|
|
inverse_kinematics(rtarget);
|
|
|
|
|
ADJUST_DELTA(rtarget);
|
|
|
|
|
const float adiff = abs(delta[A_AXIS] - oldA),
|
|
|
|
|
bdiff = abs(delta[B_AXIS] - oldB);
|
|
|
|
|
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], logical[E_AXIS], max(adiff, bdiff) * feed_factor, active_extruder);
|
|
|
|
|
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], max(adiff, bdiff) * feed_factor, active_extruder);
|
|
|
|
|
#else
|
|
|
|
|
planner.buffer_line_kinematic(ltarget, _feedrate_mm_s, active_extruder);
|
|
|
|
|
planner.buffer_line_kinematic(rtarget, _feedrate_mm_s, active_extruder);
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
return false;
|
|
|
|
|
@ -687,7 +683,7 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
|
|
|
|
|
|
|
|
|
|
float x_home_pos(const int extruder) {
|
|
|
|
|
if (extruder == 0)
|
|
|
|
|
return LOGICAL_X_POSITION(base_home_pos(X_AXIS));
|
|
|
|
|
return base_home_pos(X_AXIS);
|
|
|
|
|
else
|
|
|
|
|
/**
|
|
|
|
|
* In dual carriage mode the extruder offset provides an override of the
|
|
|
|
|
@ -695,7 +691,7 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
|
|
|
|
|
* This allows soft recalibration of the second extruder home position
|
|
|
|
|
* without firmware reflash (through the M218 command).
|
|
|
|
|
*/
|
|
|
|
|
return LOGICAL_X_POSITION(hotend_offset[X_AXIS][1] > 0 ? hotend_offset[X_AXIS][1] : X2_HOME_POS);
|
|
|
|
|
return hotend_offset[X_AXIS][1] > 0 ? hotend_offset[X_AXIS][1] : X2_HOME_POS;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
@ -740,13 +736,13 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
|
|
|
|
|
if (active_extruder == 0) {
|
|
|
|
|
#if ENABLED(DEBUG_LEVELING_FEATURE)
|
|
|
|
|
if (DEBUGGING(LEVELING)) {
|
|
|
|
|
SERIAL_ECHOPAIR("Set planner X", LOGICAL_X_POSITION(inactive_extruder_x_pos));
|
|
|
|
|
SERIAL_ECHOPAIR("Set planner X", inactive_extruder_x_pos);
|
|
|
|
|
SERIAL_ECHOLNPAIR(" ... Line to X", current_position[X_AXIS] + duplicate_extruder_x_offset);
|
|
|
|
|
}
|
|
|
|
|
#endif
|
|
|
|
|
// move duplicate extruder into correct duplication position.
|
|
|
|
|
planner.set_position_mm(
|
|
|
|
|
LOGICAL_X_POSITION(inactive_extruder_x_pos),
|
|
|
|
|
inactive_extruder_x_pos,
|
|
|
|
|
current_position[Y_AXIS],
|
|
|
|
|
current_position[Z_AXIS],
|
|
|
|
|
current_position[E_AXIS]
|
|
|
|
|
@ -970,7 +966,7 @@ void set_axis_is_at_home(const AxisEnum axis) {
|
|
|
|
|
#if ENABLED(MORGAN_SCARA)
|
|
|
|
|
scara_set_axis_is_at_home(axis);
|
|
|
|
|
#else
|
|
|
|
|
current_position[axis] = LOGICAL_POSITION(base_home_pos(axis), axis);
|
|
|
|
|
current_position[axis] = base_home_pos(axis);
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
|
Scott Lahteine
7 years ago