bissen21/Marlin_Firmware
1
0
Fork 0
Code Issues Pull Requests Projects Releases 4 Wiki Activity

Add custom types for position (#15204)

Browse Source
This commit is contained in:
Scott Lahteine
2019-09-29 04:25:39 -05:00
committed by GitHub
parent 43d6e9fa43
commit 50e4545255
227 changed files with 3147 additions and 3264 deletions
Download Patch File Download Diff File Expand all files Collapse all files

326
Marlin/src/module/motion.cpp
View File

@ -70,7 +70,7 @@
#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE)
#include "../core/debug_out.h"
#define XYZ_CONSTS(type, array, CONFIG) const PROGMEM type array##_P[XYZ] = { X_##CONFIG, Y_##CONFIG, Z_##CONFIG }
#define XYZ_CONSTS(T, NAME, OPT) const PROGMEM XYZval<T> NAME##_P = { X_##OPT, Y_##OPT, Z_##OPT }
XYZ_CONSTS(float, base_min_pos, MIN_POS);
XYZ_CONSTS(float, base_max_pos, MAX_POS);
@ -99,7 +99,7 @@ bool relative_mode; // = false;
* Used by 'line_to_current_position' to do a move after changing it.
* Used by 'sync_plan_position' to update 'planner.position'.
*/
float current_position[XYZE] = { X_HOME_POS, Y_HOME_POS, Z_HOME_POS };
xyze_pos_t current_position = { X_HOME_POS, Y_HOME_POS, Z_HOME_POS };
/**
* Cartesian Destination
@ -107,7 +107,7 @@ float current_position[XYZE] = { X_HOME_POS, Y_HOME_POS, Z_HOME_POS };
* and expected by functions like 'prepare_move_to_destination'.
* G-codes can set destination using 'get_destination_from_command'
*/
float destination[XYZE]; // = { 0 }
xyze_pos_t destination; // {0}
// The active extruder (tool). Set with T<extruder> command.
#if EXTRUDERS > 1
@ -116,16 +116,17 @@ float destination[XYZE]; // = { 0 }
// Extruder offsets
#if HAS_HOTEND_OFFSET
float hotend_offset[XYZ][HOTENDS]; // Initialized by settings.load()
xyz_pos_t hotend_offset[HOTENDS]; // Initialized by settings.load()
void reset_hotend_offsets() {
constexpr float tmp[XYZ][HOTENDS] = { HOTEND_OFFSET_X, HOTEND_OFFSET_Y, HOTEND_OFFSET_Z };
static_assert(
tmp[X_AXIS][0] == 0 && tmp[Y_AXIS][0] == 0 && tmp[Z_AXIS][0] == 0,
!tmp[X_AXIS][0] && !tmp[Y_AXIS][0] && !tmp[Z_AXIS][0],
"Offsets for the first hotend must be 0.0."
);
LOOP_XYZ(i) HOTEND_LOOP() hotend_offset[i][e] = tmp[i][e];
// Transpose from [XYZ][HOTENDS] to [HOTENDS][XYZ]
HOTEND_LOOP() LOOP_XYZ(a) hotend_offset[e][a] = tmp[a][e];
#if ENABLED(DUAL_X_CARRIAGE)
hotend_offset[X_AXIS][1] = _MAX(X2_HOME_POS, X2_MAX_POS);
hotend_offset[1].x = _MAX(X2_HOME_POS, X2_MAX_POS);
#endif
}
#endif
@ -148,14 +149,14 @@ const feedRate_t homing_feedrate_mm_s[XYZ] PROGMEM = {
};
// Cartesian conversion result goes here:
float cartes[XYZ];
xyz_pos_t cartes;
#if IS_KINEMATIC
float delta[ABC];
abc_pos_t delta;
#if HAS_SCARA_OFFSET
float scara_home_offset[ABC];
abc_pos_t scara_home_offset;
#endif
#if HAS_SOFTWARE_ENDSTOPS
@ -176,16 +177,16 @@ float cartes[XYZ];
*/
#if HAS_POSITION_SHIFT
// The distance that XYZ has been offset by G92. Reset by G28.
float position_shift[XYZ] = { 0 };
xyz_pos_t position_shift{0};
#endif
#if HAS_HOME_OFFSET
// This offset is added to the configured home position.
// Set by M206, M428, or menu item. Saved to EEPROM.
float home_offset[XYZ] = { 0 };
xyz_pos_t home_offset{0};
#endif
#if HAS_HOME_OFFSET && HAS_POSITION_SHIFT
// The above two are combined to save on computes
float workspace_offset[XYZ] = { 0 };
xyz_pos_t workspace_offset{0};
#endif
#if HAS_ABL_NOT_UBL
@ -196,10 +197,8 @@ float cartes[XYZ];
* Output the current position to serial
*/
void report_current_position() {
SERIAL_ECHOPAIR("X:", LOGICAL_X_POSITION(current_position[X_AXIS]));
SERIAL_ECHOPAIR(" Y:", LOGICAL_Y_POSITION(current_position[Y_AXIS]));
SERIAL_ECHOPAIR(" Z:", LOGICAL_Z_POSITION(current_position[Z_AXIS]));
SERIAL_ECHOPAIR(" E:", current_position[E_AXIS]);
const xyz_pos_t lpos = current_position.asLogical();
SERIAL_ECHOPAIR("X:", lpos.x, " Y:", lpos.y, " Z:", lpos.z, " E:", current_position.e);
stepper.report_positions();
@ -216,10 +215,10 @@ void report_current_position() {
*/
void sync_plan_position() {
if (DEBUGGING(LEVELING)) DEBUG_POS("sync_plan_position", current_position);
planner.set_position_mm(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
planner.set_position_mm(current_position);
}
void sync_plan_position_e() { planner.set_e_position_mm(current_position[E_AXIS]); }
void sync_plan_position_e() { planner.set_e_position_mm(current_position.e); }
/**
* Get the stepper positions in the cartes[] array.
@ -244,10 +243,9 @@ void get_cartesian_from_steppers() {
planner.get_axis_position_degrees(B_AXIS)
);
#else
cartes[X_AXIS] = planner.get_axis_position_mm(X_AXIS);
cartes[Y_AXIS] = planner.get_axis_position_mm(Y_AXIS);
cartes.set(planner.get_axis_position_mm(X_AXIS), planner.get_axis_position_mm(Y_AXIS));
#endif
cartes[Z_AXIS] = planner.get_axis_position_mm(Z_AXIS);
cartes.z = planner.get_axis_position_mm(Z_AXIS);
#endif
}
@ -266,16 +264,16 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
get_cartesian_from_steppers();
#if HAS_POSITION_MODIFIERS
float pos[XYZE] = { cartes[X_AXIS], cartes[Y_AXIS], cartes[Z_AXIS], current_position[E_AXIS] };
xyze_pos_t pos = { cartes.x, cartes.y, cartes.z, current_position.e };
planner.unapply_modifiers(pos
#if HAS_LEVELING
, true
#endif
);
const float (&cartes)[XYZE] = pos;
xyze_pos_t &cartes = pos;
#endif
if (axis == ALL_AXES)
COPY(current_position, cartes);
current_position = cartes;
else
current_position[axis] = cartes[axis];
}
@ -300,16 +298,12 @@ void line_to_current_position(const feedRate_t &fr_mm_s/*=feedrate_mm_s*/) {
// UBL segmented line will do Z-only moves in single segment
ubl.line_to_destination_segmented(scaled_fr_mm_s);
#else
if ( current_position[X_AXIS] == destination[X_AXIS]
&& current_position[Y_AXIS] == destination[Y_AXIS]
&& current_position[Z_AXIS] == destination[Z_AXIS]
&& current_position[E_AXIS] == destination[E_AXIS]
) return;
if (current_position == destination) return;
planner.buffer_line(destination, scaled_fr_mm_s, active_extruder);
#endif
set_current_from_destination();
current_position = destination;
}
#endif // IS_KINEMATIC
@ -355,39 +349,36 @@ void do_blocking_move_to(const float rx, const float ry, const float rz, const f
REMEMBER(fr, feedrate_mm_s, xy_feedrate);
set_destination_from_current(); // sync destination at the start
destination = current_position; // sync destination at the start
if (DEBUGGING(LEVELING)) DEBUG_POS("set_destination_from_current", destination);
if (DEBUGGING(LEVELING)) DEBUG_POS("destination = current_position", destination);
// when in the danger zone
if (current_position[Z_AXIS] > delta_clip_start_height) {
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_internal_fast_move_to_destination(); // set_current_from_destination()
if (current_position.z > delta_clip_start_height) {
if (rz > delta_clip_start_height) { // staying in the danger zone
destination.set(rx, ry, rz); // move directly (uninterpolated)
prepare_internal_fast_move_to_destination(); // set current_position from destination
if (DEBUGGING(LEVELING)) DEBUG_POS("danger zone move", current_position);
return;
}
destination[Z_AXIS] = delta_clip_start_height;
prepare_internal_fast_move_to_destination(); // set_current_from_destination()
destination.z = delta_clip_start_height;
prepare_internal_fast_move_to_destination(); // set current_position from destination
if (DEBUGGING(LEVELING)) DEBUG_POS("zone border move", current_position);
}
if (rz > current_position[Z_AXIS]) { // raising?
destination[Z_AXIS] = rz;
prepare_internal_fast_move_to_destination(z_feedrate); // set_current_from_destination()
if (rz > current_position.z) { // raising?
destination.z = rz;
prepare_internal_fast_move_to_destination(z_feedrate); // set current_position from destination
if (DEBUGGING(LEVELING)) DEBUG_POS("z raise move", current_position);
}
destination[X_AXIS] = rx;
destination[Y_AXIS] = ry;
prepare_internal_move_to_destination(); // set_current_from_destination()
destination.set(rx, ry);
prepare_internal_move_to_destination(); // set current_position from destination
if (DEBUGGING(LEVELING)) DEBUG_POS("xy move", current_position);
if (rz < current_position[Z_AXIS]) { // lowering?
destination[Z_AXIS] = rz;
prepare_fast_move_to_destination(z_feedrate); // set_current_from_destination()
if (rz < current_position.z) { // lowering?
destination.z = rz;
prepare_internal_fast_move_to_destination(z_feedrate); // set current_position from destination
if (DEBUGGING(LEVELING)) DEBUG_POS("z lower move", current_position);
}
@ -395,39 +386,37 @@ void do_blocking_move_to(const float rx, const float ry, const float rz, const f
if (!position_is_reachable(rx, ry)) return;
set_destination_from_current();
destination = current_position;
// If Z needs to raise, do it before moving XY
if (destination[Z_AXIS] < rz) {
destination[Z_AXIS] = rz;
if (destination.z < rz) {
destination.z = rz;
prepare_internal_fast_move_to_destination(z_feedrate);
}
destination[X_AXIS] = rx;
destination[Y_AXIS] = ry;
destination.set(rx, ry);
prepare_internal_fast_move_to_destination(xy_feedrate);
// If Z needs to lower, do it after moving XY
if (destination[Z_AXIS] > rz) {
destination[Z_AXIS] = rz;
if (destination.z > rz) {
destination.z = rz;
prepare_internal_fast_move_to_destination(z_feedrate);
}
#else
// If Z needs to raise, do it before moving XY
if (current_position[Z_AXIS] < rz) {
current_position[Z_AXIS] = rz;
if (current_position.z < rz) {
current_position.z = rz;
line_to_current_position(z_feedrate);
}
current_position[X_AXIS] = rx;
current_position[Y_AXIS] = ry;
current_position.set(rx, ry);
line_to_current_position(xy_feedrate);
// If Z needs to lower, do it after moving XY
if (current_position[Z_AXIS] > rz) {
current_position[Z_AXIS] = rz;
if (current_position.z > rz) {
current_position.z = rz;
line_to_current_position(z_feedrate);
}
@ -438,16 +427,16 @@ void do_blocking_move_to(const float rx, const float ry, const float rz, const f
planner.synchronize();
}
void do_blocking_move_to_x(const float &rx, const feedRate_t &fr_mm_s/*=0.0*/) {
do_blocking_move_to(rx, current_position[Y_AXIS], current_position[Z_AXIS], fr_mm_s);
do_blocking_move_to(rx, current_position.y, current_position.z, fr_mm_s);
}
void do_blocking_move_to_y(const float &ry, const feedRate_t &fr_mm_s/*=0.0*/) {
do_blocking_move_to(current_position[X_AXIS], ry, current_position[Z_AXIS], fr_mm_s);
do_blocking_move_to(current_position.x, ry, current_position.z, fr_mm_s);
}
void do_blocking_move_to_z(const float &rz, const feedRate_t &fr_mm_s/*=0.0*/) {
do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], rz, fr_mm_s);
do_blocking_move_to(current_position.x, current_position.y, rz, fr_mm_s);
}
void do_blocking_move_to_xy(const float &rx, const float &ry, const feedRate_t &fr_mm_s/*=0.0*/) {
do_blocking_move_to(rx, ry, current_position[Z_AXIS], fr_mm_s);
do_blocking_move_to(rx, ry, current_position.z, fr_mm_s);
}
//
@ -474,7 +463,10 @@ void restore_feedrate_and_scaling() {
bool soft_endstops_enabled = true;
// Software Endstops are based on the configured limits.
axis_limits_t soft_endstop[XYZ] = { { X_MIN_BED, X_MAX_BED }, { Y_MIN_BED, Y_MAX_BED }, { Z_MIN_POS, Z_MAX_POS } };
axis_limits_t soft_endstop = {
{ X_MIN_POS, Y_MIN_POS, Z_MIN_POS },
{ X_MAX_POS, Y_MAX_POS, Z_MAX_POS }
};
/**
* Software endstops can be used to monitor the open end of
@ -496,33 +488,33 @@ void restore_feedrate_and_scaling() {
if (axis == X_AXIS) {
// In Dual X mode hotend_offset[X] is T1's home position
const float dual_max_x = _MAX(hotend_offset[X_AXIS][1], X2_MAX_POS);
const float dual_max_x = _MAX(hotend_offset[1].x, X2_MAX_POS);
if (new_tool_index != 0) {
// T1 can move from X2_MIN_POS to X2_MAX_POS or X2 home position (whichever is larger)
soft_endstop[X_AXIS].min = X2_MIN_POS;
soft_endstop[X_AXIS].max = dual_max_x;
soft_endstop.min.x = X2_MIN_POS;
soft_endstop.max.x = dual_max_x;
}
else if (dxc_is_duplicating()) {
// In Duplication Mode, T0 can move as far left as X1_MIN_POS
// but not so far to the right that T1 would move past the end
soft_endstop[X_AXIS].min = X1_MIN_POS;
soft_endstop[X_AXIS].max = _MIN(X1_MAX_POS, dual_max_x - duplicate_extruder_x_offset);
soft_endstop.min.x = X1_MIN_POS;
soft_endstop.max.x = _MIN(X1_MAX_POS, dual_max_x - duplicate_extruder_x_offset);
}
else {
// In other modes, T0 can move from X1_MIN_POS to X1_MAX_POS
soft_endstop[X_AXIS].min = X1_MIN_POS;
soft_endstop[X_AXIS].max = X1_MAX_POS;
soft_endstop.min.x = X1_MIN_POS;
soft_endstop.max.x = X1_MAX_POS;
}
}
#elif ENABLED(DELTA)
soft_endstop[axis].min = base_min_pos(axis);
soft_endstop[axis].max = (axis == Z_AXIS ? delta_height
soft_endstop.min[axis] = base_min_pos(axis);
soft_endstop.max[axis] = (axis == Z_AXIS ? delta_height
#if HAS_BED_PROBE
- probe_offset[Z_AXIS]
- probe_offset.z
#endif
: base_max_pos(axis));
@ -530,11 +522,11 @@ void restore_feedrate_and_scaling() {
case X_AXIS:
case Y_AXIS:
// Get a minimum radius for clamping
delta_max_radius = _MIN(ABS(_MAX(soft_endstop[X_AXIS].min, soft_endstop[Y_AXIS].min)), soft_endstop[X_AXIS].max, soft_endstop[Y_AXIS].max);
delta_max_radius = _MIN(ABS(_MAX(soft_endstop.min.x, soft_endstop.min.y)), soft_endstop.max.x, soft_endstop.max.y);
delta_max_radius_2 = sq(delta_max_radius);
break;
case Z_AXIS:
delta_clip_start_height = soft_endstop[axis].max - delta_safe_distance_from_top();
delta_clip_start_height = soft_endstop.max[axis] - delta_safe_distance_from_top();
default: break;
}
@ -544,25 +536,25 @@ void restore_feedrate_and_scaling() {
// the movement limits must be shifted by the tool offset to
// retain the same physical limit when other tools are selected.
if (old_tool_index != new_tool_index) {
const float offs = hotend_offset[axis][new_tool_index] - hotend_offset[axis][old_tool_index];
soft_endstop[axis].min += offs;
soft_endstop[axis].max += offs;
const float offs = hotend_offset[new_tool_index][axis] - hotend_offset[old_tool_index][axis];
soft_endstop.min[axis] += offs;
soft_endstop.max[axis] += offs;
}
else {
const float offs = hotend_offset[axis][active_extruder];
soft_endstop[axis].min = base_min_pos(axis) + offs;
soft_endstop[axis].max = base_max_pos(axis) + offs;
const float offs = hotend_offset[active_extruder][axis];
soft_endstop.min[axis] = base_min_pos(axis) + offs;
soft_endstop.max[axis] = base_max_pos(axis) + offs;
}
#else
soft_endstop[axis].min = base_min_pos(axis);
soft_endstop[axis].max = base_max_pos(axis);
soft_endstop.min[axis] = base_min_pos(axis);
soft_endstop.max[axis] = base_max_pos(axis);
#endif
if (DEBUGGING(LEVELING))
SERIAL_ECHOLNPAIR("Axis ", axis_codes[axis], " min:", soft_endstop[axis].min, " max:", soft_endstop[axis].max);
SERIAL_ECHOLNPAIR("Axis ", axis_codes[axis], " min:", soft_endstop.min[axis], " max:", soft_endstop.max[axis]);
}
/**
@ -571,7 +563,7 @@ void restore_feedrate_and_scaling() {
* For DELTA/SCARA the XY constraint is based on the smallest
* radius within the set software endstops.
*/
void apply_motion_limits(float target[XYZ]) {
void apply_motion_limits(xyz_pos_t &target) {
if (!soft_endstops_enabled || !all_axes_homed()) return;
@ -579,41 +571,38 @@ void restore_feedrate_and_scaling() {
#if HAS_HOTEND_OFFSET && ENABLED(DELTA)
// The effector center position will be the target minus the hotend offset.
const float offx = hotend_offset[X_AXIS][active_extruder], offy = hotend_offset[Y_AXIS][active_extruder];
const xy_pos_t offs = hotend_offset[active_extruder];
#else
// SCARA needs to consider the angle of the arm through the entire move, so for now use no tool offset.
constexpr float offx = 0, offy = 0;
constexpr xy_pos_t offs{0};
#endif
const float dist_2 = HYPOT2(target[X_AXIS] - offx, target[Y_AXIS] - offy);
if (dist_2 > delta_max_radius_2) {
const float ratio = (delta_max_radius) / SQRT(dist_2); // 200 / 300 = 0.66
target[X_AXIS] *= ratio;
target[Y_AXIS] *= ratio;
}
const float dist_2 = HYPOT2(target.x - offs.x, target.y - offs.y);
if (dist_2 > delta_max_radius_2)
target *= delta_max_radius / SQRT(dist_2); // 200 / 300 = 0.66
#else
#if !HAS_SOFTWARE_ENDSTOPS || ENABLED(MIN_SOFTWARE_ENDSTOP_X)
NOLESS(target[X_AXIS], soft_endstop[X_AXIS].min);
NOLESS(target.x, soft_endstop.min.x);
#endif
#if !HAS_SOFTWARE_ENDSTOPS || ENABLED(MAX_SOFTWARE_ENDSTOP_X)
NOMORE(target[X_AXIS], soft_endstop[X_AXIS].max);
NOMORE(target.x, soft_endstop.max.x);
#endif
#if !HAS_SOFTWARE_ENDSTOPS || ENABLED(MIN_SOFTWARE_ENDSTOP_Y)
NOLESS(target[Y_AXIS], soft_endstop[Y_AXIS].min);
NOLESS(target.y, soft_endstop.min.y);
#endif
#if !HAS_SOFTWARE_ENDSTOPS || ENABLED(MAX_SOFTWARE_ENDSTOP_Y)
NOMORE(target[Y_AXIS], soft_endstop[Y_AXIS].max);
NOMORE(target.y, soft_endstop.max.y);
#endif
#endif
#if !HAS_SOFTWARE_ENDSTOPS || ENABLED(MIN_SOFTWARE_ENDSTOP_Z)
NOLESS(target[Z_AXIS], soft_endstop[Z_AXIS].min);
NOLESS(target.z, soft_endstop.min.z);
#endif
#if !HAS_SOFTWARE_ENDSTOPS || ENABLED(MAX_SOFTWARE_ENDSTOP_Z)
NOMORE(target[Z_AXIS], soft_endstop[Z_AXIS].max);
NOMORE(target.z, soft_endstop.max.z);
#endif
}
@ -656,27 +645,22 @@ void restore_feedrate_and_scaling() {
// Get the top feedrate of the move in the XY plane
const float scaled_fr_mm_s = MMS_SCALED(feedrate_mm_s);
const float xdiff = destination[X_AXIS] - current_position[X_AXIS],
ydiff = destination[Y_AXIS] - current_position[Y_AXIS];
const xyze_float_t diff = destination - current_position;
// If the move is only in Z/E don't split up the move
if (!xdiff && !ydiff) {
if (!diff.x && !diff.y) {
planner.buffer_line(destination, scaled_fr_mm_s, active_extruder);
return false; // caller will update current_position
}
// Fail if attempting move outside printable radius
if (!position_is_reachable(destination[X_AXIS], destination[Y_AXIS])) return true;
// Remaining cartesian distances
const float zdiff = destination[Z_AXIS] - current_position[Z_AXIS],
ediff = destination[E_AXIS] - current_position[E_AXIS];
if (!position_is_reachable(destination)) return true;
// Get the linear distance in XYZ
float cartesian_mm = SQRT(sq(xdiff) + sq(ydiff) + sq(zdiff));
float cartesian_mm = diff.magnitude();
// If the move is very short, check the E move distance
if (UNEAR_ZERO(cartesian_mm)) cartesian_mm = ABS(ediff);
if (UNEAR_ZERO(cartesian_mm)) cartesian_mm = ABS(diff.e);
// No E move either? Game over.
if (UNEAR_ZERO(cartesian_mm)) return true;
@ -698,13 +682,8 @@ void restore_feedrate_and_scaling() {
// The approximate length of each segment
const float inv_segments = 1.0f / float(segments),
segment_distance[XYZE] = {
xdiff * inv_segments,
ydiff * inv_segments,
zdiff * inv_segments,
ediff * inv_segments
},
cartesian_segment_mm = cartesian_mm * inv_segments;
const xyze_float_t segment_distance = diff * inv_segments;
#if ENABLED(SCARA_FEEDRATE_SCALING)
const float inv_duration = scaled_fr_mm_s / cartesian_segment_mm;
@ -719,8 +698,7 @@ void restore_feedrate_and_scaling() {
//*/
// Get the current position as starting point
float raw[XYZE];
COPY(raw, current_position);
xyze_pos_t raw = current_position;
// Calculate and execute the segments
while (--segments) {
@ -732,7 +710,7 @@ void restore_feedrate_and_scaling() {
idle();
}
LOOP_XYZE(i) raw[i] += segment_distance[i];
raw += segment_distance;
if (!planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, cartesian_segment_mm
#if ENABLED(SCARA_FEEDRATE_SCALING)
@ -765,24 +743,19 @@ void restore_feedrate_and_scaling() {
*/
inline void segmented_line_to_destination(const feedRate_t &fr_mm_s, const float segment_size=LEVELED_SEGMENT_LENGTH) {
const float xdiff = destination[X_AXIS] - current_position[X_AXIS],
ydiff = destination[Y_AXIS] - current_position[Y_AXIS];
const xyze_float_t diff = destination - current_position;
// If the move is only in Z/E don't split up the move
if (!xdiff && !ydiff) {
if (!diff.x && !diff.y) {
planner.buffer_line(destination, fr_mm_s, active_extruder);
return;
}
// Remaining cartesian distances
const float zdiff = destination[Z_AXIS] - current_position[Z_AXIS],
ediff = destination[E_AXIS] - current_position[E_AXIS];
// Get the linear distance in XYZ
// If the move is very short, check the E move distance
// No E move either? Game over.
float cartesian_mm = SQRT(sq(xdiff) + sq(ydiff) + sq(zdiff));
if (UNEAR_ZERO(cartesian_mm)) cartesian_mm = ABS(ediff);
float cartesian_mm = diff.magnitude();
if (UNEAR_ZERO(cartesian_mm)) cartesian_mm = ABS(diff.e);
if (UNEAR_ZERO(cartesian_mm)) return;
// The length divided by the segment size
@ -792,13 +765,8 @@ void restore_feedrate_and_scaling() {
// The approximate length of each segment
const float inv_segments = 1.0f / float(segments),
cartesian_segment_mm = cartesian_mm * inv_segments,
segment_distance[XYZE] = {
xdiff * inv_segments,
ydiff * inv_segments,
zdiff * inv_segments,
ediff * inv_segments
};
cartesian_segment_mm = cartesian_mm * inv_segments;
const xyze_float_t segment_distance = diff * inv_segments;
#if ENABLED(SCARA_FEEDRATE_SCALING)
const float inv_duration = scaled_fr_mm_s / cartesian_segment_mm;
@ -809,8 +777,7 @@ void restore_feedrate_and_scaling() {
// SERIAL_ECHOLNPAIR(" segment_mm=", cartesian_segment_mm);
// Get the raw current position as starting point
float raw[XYZE];
COPY(raw, current_position);
xyze_pos_t raw = current_position;
// Calculate and execute the segments
while (--segments) {
@ -820,7 +787,7 @@ void restore_feedrate_and_scaling() {
next_idle_ms = millis() + 200UL;
idle();
}
LOOP_XYZE(i) raw[i] += segment_distance[i];
raw += segment_distance;
if (!planner.buffer_line(raw, fr_mm_s, active_extruder, cartesian_segment_mm
#if ENABLED(SCARA_FEEDRATE_SCALING)
, inv_duration
@ -846,12 +813,12 @@ void restore_feedrate_and_scaling() {
* When a mesh-based leveling system is active, moves are segmented
* according to the configuration of the leveling system.
*
* Returns true if current_position[] was set to destination[]
* Return true if 'current_position' was set to 'destination'
*/
inline bool prepare_move_to_destination_cartesian() {
const float scaled_fr_mm_s = MMS_SCALED(feedrate_mm_s);
#if HAS_MESH
if (planner.leveling_active && planner.leveling_active_at_z(destination[Z_AXIS])) {
if (planner.leveling_active && planner.leveling_active_at_z(destination.z)) {
#if ENABLED(AUTO_BED_LEVELING_UBL)
ubl.line_to_destination_cartesian(scaled_fr_mm_s, active_extruder); // UBL's motion routine needs to know about
return true; // all moves, including Z-only moves.
@ -863,7 +830,7 @@ void restore_feedrate_and_scaling() {
* For MBL and ABL-BILINEAR only segment moves when X or Y are involved.
* Otherwise fall through to do a direct single move.
*/
if (current_position[X_AXIS] != destination[X_AXIS] || current_position[Y_AXIS] != destination[Y_AXIS]) {
if (xy_pos_t(current_position) != xy_pos_t(destination)) {
#if ENABLED(MESH_BED_LEVELING)
mbl.line_to_destination(scaled_fr_mm_s);
#elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
@ -894,8 +861,8 @@ void restore_feedrate_and_scaling() {
DualXMode dual_x_carriage_mode = DEFAULT_DUAL_X_CARRIAGE_MODE;
float inactive_extruder_x_pos = X2_MAX_POS, // used in mode 0 & 1
raised_parked_position[XYZE], // used in mode 1
duplicate_extruder_x_offset = DEFAULT_DUPLICATION_X_OFFSET; // used in mode 2
xyz_pos_t raised_parked_position; // used in mode 1
bool active_extruder_parked = false; // used in mode 1 & 2
millis_t delayed_move_time = 0; // used in mode 1
int16_t duplicate_extruder_temp_offset = 0; // used in mode 2
@ -910,7 +877,7 @@ void restore_feedrate_and_scaling() {
* This allows soft recalibration of the second extruder home position
* without firmware reflash (through the M218 command).
*/
return hotend_offset[X_AXIS][1] > 0 ? hotend_offset[X_AXIS][1] : X2_HOME_POS;
return hotend_offset[1].x > 0 ? hotend_offset[1].x : X2_HOME_POS;
}
/**
@ -924,30 +891,30 @@ void restore_feedrate_and_scaling() {
case DXC_FULL_CONTROL_MODE:
break;
case DXC_AUTO_PARK_MODE:
if (current_position[E_AXIS] == destination[E_AXIS]) {
if (current_position.e == destination.e) {
// This is a travel move (with no extrusion)
// Skip it, but keep track of the current position
// (so it can be used as the start of the next non-travel move)
if (delayed_move_time != 0xFFFFFFFFUL) {
set_current_from_destination();
NOLESS(raised_parked_position[Z_AXIS], destination[Z_AXIS]);
current_position = destination;
NOLESS(raised_parked_position.z, destination.z);
delayed_move_time = millis();
return true;
}
}
// unpark extruder: 1) raise, 2) move into starting XY position, 3) lower
#define CUR_X current_position[X_AXIS]
#define CUR_Y current_position[Y_AXIS]
#define CUR_Z current_position[Z_AXIS]
#define CUR_E current_position[E_AXIS]
#define RAISED_X raised_parked_position[X_AXIS]
#define RAISED_Y raised_parked_position[Y_AXIS]
#define RAISED_Z raised_parked_position[Z_AXIS]
#define CUR_X current_position.x
#define CUR_Y current_position.y
#define CUR_Z current_position.z
#define CUR_E current_position.e
#define RAISED_X raised_parked_position.x
#define RAISED_Y raised_parked_position.y
#define RAISED_Z raised_parked_position.z
if ( planner.buffer_line(RAISED_X, RAISED_Y, RAISED_Z, CUR_E, planner.settings.max_feedrate_mm_s[Z_AXIS], active_extruder))
if (planner.buffer_line( CUR_X, CUR_Y, RAISED_Z, CUR_E, PLANNER_XY_FEEDRATE(), active_extruder))
planner.buffer_line( CUR_X, CUR_Y, CUR_Z, CUR_E, planner.settings.max_feedrate_mm_s[Z_AXIS], active_extruder);
line_to_current_position(planner.settings.max_feedrate_mm_s[Z_AXIS]);
delayed_move_time = 0;
active_extruder_parked = false;
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Clear active_extruder_parked");
@ -955,16 +922,15 @@ void restore_feedrate_and_scaling() {
case DXC_MIRRORED_MODE:
case DXC_DUPLICATION_MODE:
if (active_extruder == 0) {
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("Set planner X", inactive_extruder_x_pos, " ... Line to X", current_position[X_AXIS] + duplicate_extruder_x_offset);
xyze_pos_t new_pos = current_position;
if (dual_x_carriage_mode == DXC_DUPLICATION_MODE)
new_pos.x += duplicate_extruder_x_offset;
else
new_pos.x = inactive_extruder_x_pos;
// move duplicate extruder into correct duplication position.
planner.set_position_mm(inactive_extruder_x_pos, current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
if (!planner.buffer_line(
dual_x_carriage_mode == DXC_DUPLICATION_MODE ? duplicate_extruder_x_offset + current_position[X_AXIS] : inactive_extruder_x_pos,
current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS],
planner.settings.max_feedrate_mm_s[X_AXIS], 1
)
) break;
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("Set planner X", inactive_extruder_x_pos, " ... Line to X", new_pos.x);
planner.set_position_mm(inactive_extruder_x_pos, current_position.y, current_position.z, current_position.e);
if (!planner.buffer_line(new_pos, planner.settings.max_feedrate_mm_s[X_AXIS], 1)) break;
planner.synchronize();
sync_plan_position();
extruder_duplication_enabled = true;
@ -987,7 +953,7 @@ void restore_feedrate_and_scaling() {
* This may result in several calls to planner.buffer_line to
* do smaller moves for DELTA, SCARA, mesh moves, etc.
*
* Make sure current_position[E] and destination[E] are good
* Make sure current_position.e and destination.e are good
* before calling or cold/lengthy extrusion may get missed.
*
* Before exit, current_position is set to destination.
@ -998,15 +964,15 @@ void prepare_move_to_destination() {
#if EITHER(PREVENT_COLD_EXTRUSION, PREVENT_LENGTHY_EXTRUDE)
if (!DEBUGGING(DRYRUN)) {
if (destination[E_AXIS] != current_position[E_AXIS]) {
if (destination.e != current_position.e) {
#if ENABLED(PREVENT_COLD_EXTRUSION)
if (thermalManager.tooColdToExtrude(active_extruder)) {
current_position[E_AXIS] = destination[E_AXIS]; // Behave as if the move really took place, but ignore E part
current_position.e = destination.e; // Behave as if the move really took place, but ignore E part
SERIAL_ECHO_MSG(MSG_ERR_COLD_EXTRUDE_STOP);
}
#endif // PREVENT_COLD_EXTRUSION
#if ENABLED(PREVENT_LENGTHY_EXTRUDE)
const float e_delta = ABS(destination[E_AXIS] - current_position[E_AXIS]) * planner.e_factor[active_extruder];
const float e_delta = ABS(destination.e - current_position.e) * planner.e_factor[active_extruder];
if (e_delta > (EXTRUDE_MAXLENGTH)) {
#if ENABLED(MIXING_EXTRUDER)
bool ignore_e = false;
@ -1018,7 +984,7 @@ void prepare_move_to_destination() {
constexpr bool ignore_e = true;
#endif
if (ignore_e) {
current_position[E_AXIS] = destination[E_AXIS]; // Behave as if the move really took place, but ignore E part
current_position.e = destination.e; // Behave as if the move really took place, but ignore E part
SERIAL_ECHO_MSG(MSG_ERR_LONG_EXTRUDE_STOP);
}
}
@ -1046,7 +1012,7 @@ void prepare_move_to_destination() {
#endif
) return;
set_current_from_destination();
current_position = destination;
}
uint8_t axes_need_homing(uint8_t axis_bits/*=0x07*/) {
@ -1293,13 +1259,13 @@ void do_homing_move(const AxisEnum axis, const float distance, const feedRate_t
current_position[axis] = distance;
line_to_current_position(real_fr_mm_s);
#else
float target[ABCE] = { planner.get_axis_position_mm(A_AXIS), planner.get_axis_position_mm(B_AXIS), planner.get_axis_position_mm(C_AXIS), planner.get_axis_position_mm(E_AXIS) };
abce_pos_t target = { planner.get_axis_position_mm(A_AXIS), planner.get_axis_position_mm(B_AXIS), planner.get_axis_position_mm(C_AXIS), planner.get_axis_position_mm(E_AXIS) };
target[axis] = 0;
planner.set_machine_position_mm(target);
target[axis] = distance;
#if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION)
const float delta_mm_cart[XYZE] = {0, 0, 0, 0};
const xyze_float_t delta_mm_cart{0};
#endif
// Set delta/cartesian axes directly
@ -1356,7 +1322,7 @@ void set_axis_is_at_home(const AxisEnum axis) {
#if ENABLED(DUAL_X_CARRIAGE)
if (axis == X_AXIS && (active_extruder == 1 || dual_x_carriage_mode == DXC_DUPLICATION_MODE)) {
current_position[X_AXIS] = x_home_pos(active_extruder);
current_position.x = x_home_pos(active_extruder);
return;
}
#endif
@ -1366,7 +1332,7 @@ void set_axis_is_at_home(const AxisEnum axis) {
#elif ENABLED(DELTA)
current_position[axis] = (axis == Z_AXIS ? delta_height
#if HAS_BED_PROBE
- probe_offset[Z_AXIS]
- probe_offset.z
#endif
: base_home_pos(axis));
#else
@ -1380,9 +1346,9 @@ void set_axis_is_at_home(const AxisEnum axis) {
if (axis == Z_AXIS) {
#if HOMING_Z_WITH_PROBE
current_position[Z_AXIS] -= probe_offset[Z_AXIS];
current_position.z -= probe_offset.z;
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("*** Z HOMED WITH PROBE (Z_MIN_PROBE_USES_Z_MIN_ENDSTOP_PIN) ***\n> probe_offset[Z_AXIS] = ", probe_offset[Z_AXIS]);
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("*** Z HOMED WITH PROBE (Z_MIN_PROBE_USES_Z_MIN_ENDSTOP_PIN) ***\n> probe_offset.z = ", probe_offset.z);
#else
@ -1677,7 +1643,7 @@ void homeaxis(const AxisEnum axis) {
#endif
#ifdef HOMING_BACKOFF_MM
constexpr float endstop_backoff[XYZ] = HOMING_BACKOFF_MM;
constexpr xyz_float_t endstop_backoff = HOMING_BACKOFF_MM;
const float backoff_mm = endstop_backoff[
#if ENABLED(DELTA)
Z_AXIS