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

147
Marlin/src/module/configuration_store.cpp
View File

@ -52,7 +52,7 @@
#include "temperature.h"
#include "../lcd/ultralcd.h"
#include "../core/language.h"
#include "../libs/vector_3.h"
#include "../libs/vector_3.h" // for matrix_3x3
#include "../gcode/gcode.h"
#include "../Marlin.h"
@ -146,13 +146,13 @@ typedef struct SettingsDataStruct {
planner_settings_t planner_settings;
float planner_max_jerk[XYZE], // M205 XYZE planner.max_jerk[XYZE]
planner_junction_deviation_mm; // M205 J planner.junction_deviation_mm
xyze_float_t planner_max_jerk; // M205 XYZE planner.max_jerk
float planner_junction_deviation_mm; // M205 J planner.junction_deviation_mm
float home_offset[XYZ]; // M206 XYZ / M665 TPZ
xyz_pos_t home_offset; // M206 XYZ / M665 TPZ
#if HAS_HOTEND_OFFSET
float hotend_offset[XYZ][HOTENDS - 1]; // M218 XYZ
xyz_pos_t hotend_offset[HOTENDS - 1]; // M218 XYZ
#endif
//
@ -181,7 +181,7 @@ typedef struct SettingsDataStruct {
// HAS_BED_PROBE
//
float probe_offset[XYZ];
xyz_pos_t probe_offset;
//
// ABL_PLANAR
@ -192,10 +192,9 @@ typedef struct SettingsDataStruct {
// AUTO_BED_LEVELING_BILINEAR
//
uint8_t grid_max_x, grid_max_y; // GRID_MAX_POINTS_X, GRID_MAX_POINTS_Y
int bilinear_grid_spacing[2],
bilinear_start[2]; // G29 L F
xy_int_t bilinear_grid_spacing, bilinear_start; // G29 L F
#if ENABLED(AUTO_BED_LEVELING_BILINEAR)
float z_values[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y]; // G29
bed_mesh_t z_values; // G29
#else
float z_values[3][3];
#endif
@ -220,13 +219,13 @@ typedef struct SettingsDataStruct {
// DELTA / [XYZ]_DUAL_ENDSTOPS
//
#if ENABLED(DELTA)
float delta_height, // M666 H
delta_endstop_adj[ABC], // M666 XYZ
delta_radius, // M665 R
float delta_height; // M666 H
abc_float_t delta_endstop_adj; // M666 XYZ
float delta_radius, // M665 R
delta_diagonal_rod, // M665 L
delta_segments_per_second, // M665 S
delta_calibration_radius, // M665 B
delta_tower_angle_trim[ABC]; // M665 XYZ
delta_calibration_radius; // M665 B
abc_float_t delta_tower_angle_trim; // M665 XYZ
#elif EITHER(X_DUAL_ENDSTOPS, Y_DUAL_ENDSTOPS) || Z_MULTI_ENDSTOPS
float x2_endstop_adj, // M666 X
y2_endstop_adj, // M666 Y
@ -302,7 +301,7 @@ typedef struct SettingsDataStruct {
//
// CNC_COORDINATE_SYSTEMS
//
float coordinate_system[MAX_COORDINATE_SYSTEMS][XYZ]; // G54-G59.3
xyz_pos_t coordinate_system[MAX_COORDINATE_SYSTEMS]; // G54-G59.3
//
// SKEW_CORRECTION
@ -326,7 +325,7 @@ typedef struct SettingsDataStruct {
//
// BACKLASH_COMPENSATION
//
float backlash_distance_mm[XYZ]; // M425 X Y Z
xyz_float_t backlash_distance_mm; // M425 X Y Z
uint8_t backlash_correction; // M425 F
float backlash_smoothing_mm; // M425 S
@ -355,7 +354,7 @@ uint16_t MarlinSettings::datasize() { return sizeof(SettingsData); }
#endif
void MarlinSettings::postprocess() {
const float oldpos[XYZE] = { current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS] };
xyze_pos_t oldpos = current_position;
// steps per s2 needs to be updated to agree with units per s2
planner.reset_acceleration_rates();
@ -408,7 +407,7 @@ void MarlinSettings::postprocess() {
planner.refresh_positioning();
// Various factors can change the current position
if (memcmp(oldpos, current_position, sizeof(oldpos)))
if (oldpos != current_position)
report_current_position();
}
@ -522,7 +521,7 @@ void MarlinSettings::postprocess() {
EEPROM_WRITE(dummy);
#endif
#else
const float planner_max_jerk[XYZE] = { float(DEFAULT_EJERK) };
const xyze_pos_t planner_max_jerk = { 10, 10, 0.4, float(DEFAULT_EJERK) };
EEPROM_WRITE(planner_max_jerk);
#endif
@ -544,7 +543,7 @@ void MarlinSettings::postprocess() {
EEPROM_WRITE(scara_home_offset);
#else
#if !HAS_HOME_OFFSET
const float home_offset[XYZ] = { 0 };
const xyz_pos_t home_offset{0};
#endif
EEPROM_WRITE(home_offset);
#endif
@ -552,7 +551,7 @@ void MarlinSettings::postprocess() {
#if HAS_HOTEND_OFFSET
// Skip hotend 0 which must be 0
for (uint8_t e = 1; e < HOTENDS; e++)
LOOP_XYZ(i) EEPROM_WRITE(hotend_offset[i][e]);
EEPROM_WRITE(hotend_offset[e]);
#endif
}
@ -618,7 +617,7 @@ void MarlinSettings::postprocess() {
// Probe Z Offset
//
{
_FIELD_TEST(probe_offset[Z_AXIS]);
_FIELD_TEST(probe_offset.z);
EEPROM_WRITE(probe_offset);
}
@ -653,7 +652,7 @@ void MarlinSettings::postprocess() {
#else
// For disabled Bilinear Grid write an empty 3x3 grid
const uint8_t grid_max_x = 3, grid_max_y = 3;
const int bilinear_start[2] = { 0 }, bilinear_grid_spacing[2] = { 0 };
const xy_int_t bilinear_start{0}, bilinear_grid_spacing{0};
dummy = 0;
EEPROM_WRITE(grid_max_x);
EEPROM_WRITE(grid_max_y);
@ -1033,7 +1032,7 @@ void MarlinSettings::postprocess() {
// TMC StallGuard threshold
//
{
tmc_sgt_t tmc_sgt = { 0 };
tmc_sgt_t tmc_sgt{0};
#if USE_SENSORLESS
#if X_SENSORLESS
tmc_sgt.X = stepperX.homing_threshold();
@ -1138,8 +1137,8 @@ void MarlinSettings::postprocess() {
#if HAS_MOTOR_CURRENT_PWM
EEPROM_WRITE(stepper.motor_current_setting);
#else
const uint32_t dummyui32[XYZ] = { 0 };
EEPROM_WRITE(dummyui32);
const xyz_ulong_t no_current{0};
EEPROM_WRITE(no_current);
#endif
}
@ -1152,7 +1151,7 @@ void MarlinSettings::postprocess() {
#if ENABLED(CNC_COORDINATE_SYSTEMS)
EEPROM_WRITE(gcode.coordinate_system);
#else
const float coordinate_system[MAX_COORDINATE_SYSTEMS][XYZ] = { { 0 } };
const xyz_pos_t coordinate_system[MAX_COORDINATE_SYSTEMS] = { { 0 } };
EEPROM_WRITE(coordinate_system);
#endif
@ -1189,10 +1188,10 @@ void MarlinSettings::postprocess() {
//
{
#if ENABLED(BACKLASH_GCODE)
const float (&backlash_distance_mm)[XYZ] = backlash.distance_mm;
const xyz_float_t &backlash_distance_mm = backlash.distance_mm;
const uint8_t &backlash_correction = backlash.correction;
#else
const float backlash_distance_mm[XYZ] = { 0 };
const xyz_float_t backlash_distance_mm{0};
const uint8_t backlash_correction = 0;
#endif
#if ENABLED(BACKLASH_GCODE) && defined(BACKLASH_SMOOTHING_MM)
@ -1341,7 +1340,7 @@ void MarlinSettings::postprocess() {
EEPROM_READ(scara_home_offset);
#else
#if !HAS_HOME_OFFSET
float home_offset[XYZ];
xyz_pos_t home_offset;
#endif
EEPROM_READ(home_offset);
#endif
@ -1354,7 +1353,7 @@ void MarlinSettings::postprocess() {
#if HAS_HOTEND_OFFSET
// Skip hotend 0 which must be 0
for (uint8_t e = 1; e < HOTENDS; e++)
LOOP_XYZ(i) EEPROM_READ(hotend_offset[i][e]);
EEPROM_READ(hotend_offset[e]);
#endif
}
@ -1418,12 +1417,11 @@ void MarlinSettings::postprocess() {
// Probe Z Offset
//
{
_FIELD_TEST(probe_offset[Z_AXIS]);
_FIELD_TEST(probe_offset);
#if HAS_BED_PROBE
float (&zpo)[XYZ] = probe_offset;
xyz_pos_t &zpo = probe_offset;
#else
float zpo[XYZ];
xyz_pos_t zpo;
#endif
EEPROM_READ(zpo);
}
@ -1457,7 +1455,7 @@ void MarlinSettings::postprocess() {
#endif // AUTO_BED_LEVELING_BILINEAR
{
// Skip past disabled (or stale) Bilinear Grid data
int bgs[2], bs[2];
xy_int_t bgs, bs;
EEPROM_READ(bgs);
EEPROM_READ(bs);
for (uint16_t q = grid_max_x * grid_max_y; q--;) EEPROM_READ(dummy);
@ -1940,7 +1938,7 @@ void MarlinSettings::postprocess() {
if (!validating) (void)gcode.select_coordinate_system(-1); // Go back to machine space
EEPROM_READ(gcode.coordinate_system);
#else
float coordinate_system[MAX_COORDINATE_SYSTEMS][XYZ];
xyz_pos_t coordinate_system[MAX_COORDINATE_SYSTEMS];
EEPROM_READ(coordinate_system);
#endif
}
@ -1989,7 +1987,7 @@ void MarlinSettings::postprocess() {
//
{
#if ENABLED(BACKLASH_GCODE)
float (&backlash_distance_mm)[XYZ] = backlash.distance_mm;
xyz_float_t &backlash_distance_mm = backlash.distance_mm;
uint8_t &backlash_correction = backlash.correction;
#else
float backlash_distance_mm[XYZ];
@ -2231,11 +2229,9 @@ void MarlinSettings::reset() {
#ifndef DEFAULT_ZJERK
#define DEFAULT_ZJERK 0
#endif
planner.max_jerk[X_AXIS] = DEFAULT_XJERK;
planner.max_jerk[Y_AXIS] = DEFAULT_YJERK;
planner.max_jerk[Z_AXIS] = DEFAULT_ZJERK;
planner.max_jerk.set(DEFAULT_XJERK, DEFAULT_YJERK, DEFAULT_ZJERK);
#if !BOTH(JUNCTION_DEVIATION, LIN_ADVANCE)
planner.max_jerk[E_AXIS] = DEFAULT_EJERK;
planner.max_jerk.e = DEFAULT_EJERK;
#endif
#endif
@ -2244,9 +2240,9 @@ void MarlinSettings::reset() {
#endif
#if HAS_SCARA_OFFSET
ZERO(scara_home_offset);
scara_home_offset.reset();
#elif HAS_HOME_OFFSET
ZERO(home_offset);
home_offset.reset();
#endif
#if HAS_HOTEND_OFFSET
@ -2277,17 +2273,16 @@ void MarlinSettings::reset() {
toolchange_settings.retract_speed = TOOLCHANGE_FIL_SWAP_RETRACT_SPEED;
#endif
#if ENABLED(TOOLCHANGE_PARK)
toolchange_settings.change_point = TOOLCHANGE_PARK_XY;
constexpr xyz_pos_t tpxy = TOOLCHANGE_PARK_XY;
toolchange_settings.change_point = tpxy;
#endif
toolchange_settings.z_raise = TOOLCHANGE_ZRAISE;
#endif
#if ENABLED(BACKLASH_GCODE)
backlash.correction = (BACKLASH_CORRECTION) * 255;
constexpr float tmp[XYZ] = BACKLASH_DISTANCE_MM;
backlash.distance_mm[X_AXIS] = tmp[X_AXIS];
backlash.distance_mm[Y_AXIS] = tmp[Y_AXIS];
backlash.distance_mm[Z_AXIS] = tmp[Z_AXIS];
constexpr xyz_float_t tmp = BACKLASH_DISTANCE_MM;
backlash.distance_mm = tmp;
#ifdef BACKLASH_SMOOTHING_MM
backlash.smoothing_mm = BACKLASH_SMOOTHING_MM;
#endif
@ -2346,14 +2341,14 @@ void MarlinSettings::reset() {
//
#if ENABLED(DELTA)
const float adj[ABC] = DELTA_ENDSTOP_ADJ, dta[ABC] = DELTA_TOWER_ANGLE_TRIM;
const abc_float_t adj = DELTA_ENDSTOP_ADJ, dta = DELTA_TOWER_ANGLE_TRIM;
delta_height = DELTA_HEIGHT;
COPY(delta_endstop_adj, adj);
delta_endstop_adj = adj;
delta_radius = DELTA_RADIUS;
delta_diagonal_rod = DELTA_DIAGONAL_ROD;
delta_segments_per_second = DELTA_SEGMENTS_PER_SECOND;
delta_calibration_radius = DELTA_CALIBRATION_RADIUS;
COPY(delta_tower_angle_trim, dta);
delta_tower_angle_trim = dta;
#elif EITHER(X_DUAL_ENDSTOPS, Y_DUAL_ENDSTOPS) || Z_MULTI_ENDSTOPS
@ -2769,11 +2764,11 @@ void MarlinSettings::reset() {
, " J", LINEAR_UNIT(planner.junction_deviation_mm)
#endif
#if HAS_CLASSIC_JERK
, " X", LINEAR_UNIT(planner.max_jerk[X_AXIS])
, " Y", LINEAR_UNIT(planner.max_jerk[Y_AXIS])
, " Z", LINEAR_UNIT(planner.max_jerk[Z_AXIS])
, " X", LINEAR_UNIT(planner.max_jerk.x)
, " Y", LINEAR_UNIT(planner.max_jerk.y)
, " Z", LINEAR_UNIT(planner.max_jerk.z)
#if !BOTH(JUNCTION_DEVIATION, LIN_ADVANCE)
, " E", LINEAR_UNIT(planner.max_jerk[E_AXIS])
, " E", LINEAR_UNIT(planner.max_jerk.e)
#endif
#endif
);
@ -2783,10 +2778,10 @@ void MarlinSettings::reset() {
CONFIG_ECHO_START();
SERIAL_ECHOLNPAIR(" M206"
#if IS_CARTESIAN
" X", LINEAR_UNIT(home_offset[X_AXIS]),
" Y", LINEAR_UNIT(home_offset[Y_AXIS]),
" X", LINEAR_UNIT(home_offset.x),
" Y", LINEAR_UNIT(home_offset.y),
#endif
" Z", LINEAR_UNIT(home_offset[Z_AXIS])
" Z", LINEAR_UNIT(home_offset.z)
);
#endif
@ -2796,9 +2791,9 @@ void MarlinSettings::reset() {
for (uint8_t e = 1; e < HOTENDS; e++) {
SERIAL_ECHOPAIR(
" M218 T", (int)e,
" X", LINEAR_UNIT(hotend_offset[X_AXIS][e]), " Y", LINEAR_UNIT(hotend_offset[Y_AXIS][e])
" X", LINEAR_UNIT(hotend_offset[e].x), " Y", LINEAR_UNIT(hotend_offset[e].y)
);
SERIAL_ECHOLNPAIR_F(" Z", LINEAR_UNIT(hotend_offset[Z_AXIS][e]), 3);
SERIAL_ECHOLNPAIR_F(" Z", LINEAR_UNIT(hotend_offset[e].z), 3);
}
#endif
@ -2901,9 +2896,9 @@ void MarlinSettings::reset() {
CONFIG_ECHO_START();
SERIAL_ECHOLNPAIR(
" M665 S", delta_segments_per_second
, " P", scara_home_offset[A_AXIS]
, " T", scara_home_offset[B_AXIS]
, " Z", LINEAR_UNIT(scara_home_offset[Z_AXIS])
, " P", scara_home_offset.a
, " T", scara_home_offset.b
, " Z", LINEAR_UNIT(scara_home_offset.z)
);
#elif ENABLED(DELTA)
@ -2911,9 +2906,9 @@ void MarlinSettings::reset() {
CONFIG_ECHO_HEADING("Endstop adjustment:");
CONFIG_ECHO_START();
SERIAL_ECHOLNPAIR(
" M666 X", LINEAR_UNIT(delta_endstop_adj[A_AXIS])
, " Y", LINEAR_UNIT(delta_endstop_adj[B_AXIS])
, " Z", LINEAR_UNIT(delta_endstop_adj[C_AXIS])
" M666 X", LINEAR_UNIT(delta_endstop_adj.a)
, " Y", LINEAR_UNIT(delta_endstop_adj.b)
, " Z", LINEAR_UNIT(delta_endstop_adj.c)
);
CONFIG_ECHO_HEADING("Delta settings: L<diagonal_rod> R<radius> H<height> S<segments_per_s> B<calibration radius> XYZ<tower angle corrections>");
@ -2924,9 +2919,9 @@ void MarlinSettings::reset() {
, " H", LINEAR_UNIT(delta_height)
, " S", delta_segments_per_second
, " B", LINEAR_UNIT(delta_calibration_radius)
, " X", LINEAR_UNIT(delta_tower_angle_trim[A_AXIS])
, " Y", LINEAR_UNIT(delta_tower_angle_trim[B_AXIS])
, " Z", LINEAR_UNIT(delta_tower_angle_trim[C_AXIS])
, " X", LINEAR_UNIT(delta_tower_angle_trim.a)
, " Y", LINEAR_UNIT(delta_tower_angle_trim.b)
, " Z", LINEAR_UNIT(delta_tower_angle_trim.c)
);
#elif EITHER(X_DUAL_ENDSTOPS, Y_DUAL_ENDSTOPS) || Z_MULTI_ENDSTOPS
@ -3072,9 +3067,9 @@ void MarlinSettings::reset() {
say_units(true);
}
CONFIG_ECHO_START();
SERIAL_ECHOLNPAIR(" M851 X", LINEAR_UNIT(probe_offset[X_AXIS]),
" Y", LINEAR_UNIT(probe_offset[Y_AXIS]),
" Z", LINEAR_UNIT(probe_offset[Z_AXIS]));
SERIAL_ECHOLNPAIR(" M851 X", LINEAR_UNIT(probe_offset.x),
" Y", LINEAR_UNIT(probe_offset.y),
" Z", LINEAR_UNIT(probe_offset.z));
#endif
/**
@ -3421,9 +3416,9 @@ void MarlinSettings::reset() {
CONFIG_ECHO_START();
SERIAL_ECHOLNPAIR(
" M425 F", backlash.get_correction(),
" X", LINEAR_UNIT(backlash.distance_mm[X_AXIS]),
" Y", LINEAR_UNIT(backlash.distance_mm[Y_AXIS]),
" Z", LINEAR_UNIT(backlash.distance_mm[Z_AXIS])
" X", LINEAR_UNIT(backlash.distance_mm.x),
" Y", LINEAR_UNIT(backlash.distance_mm.y),
" Z", LINEAR_UNIT(backlash.distance_mm.z)
#ifdef BACKLASH_SMOOTHING_MM
, " S", LINEAR_UNIT(backlash.smoothing_mm)
#endif

85
Marlin/src/module/delta.cpp
View File

@ -50,17 +50,16 @@
#include "../core/debug_out.h"
// Initialized by settings.load()
float delta_height,
delta_endstop_adj[ABC] = { 0 },
delta_radius,
float delta_height;
abc_float_t delta_endstop_adj{0};
float delta_radius,
delta_diagonal_rod,
delta_segments_per_second,
delta_calibration_radius,
delta_tower_angle_trim[ABC];
float delta_tower[ABC][2],
delta_diagonal_rod_2_tower[ABC],
delta_clip_start_height = Z_MAX_POS;
delta_calibration_radius;
abc_float_t delta_tower_angle_trim;
xy_float_t delta_tower[ABC];
abc_float_t delta_diagonal_rod_2_tower;
float delta_clip_start_height = Z_MAX_POS;
float delta_safe_distance_from_top();
@ -69,17 +68,17 @@ float delta_safe_distance_from_top();
* settings have been changed (e.g., by M665).
*/
void recalc_delta_settings() {
const float trt[ABC] = DELTA_RADIUS_TRIM_TOWER,
drt[ABC] = DELTA_DIAGONAL_ROD_TRIM_TOWER;
delta_tower[A_AXIS][X_AXIS] = cos(RADIANS(210 + delta_tower_angle_trim[A_AXIS])) * (delta_radius + trt[A_AXIS]); // front left tower
delta_tower[A_AXIS][Y_AXIS] = sin(RADIANS(210 + delta_tower_angle_trim[A_AXIS])) * (delta_radius + trt[A_AXIS]);
delta_tower[B_AXIS][X_AXIS] = cos(RADIANS(330 + delta_tower_angle_trim[B_AXIS])) * (delta_radius + trt[B_AXIS]); // front right tower
delta_tower[B_AXIS][Y_AXIS] = sin(RADIANS(330 + delta_tower_angle_trim[B_AXIS])) * (delta_radius + trt[B_AXIS]);
delta_tower[C_AXIS][X_AXIS] = cos(RADIANS( 90 + delta_tower_angle_trim[C_AXIS])) * (delta_radius + trt[C_AXIS]); // back middle tower
delta_tower[C_AXIS][Y_AXIS] = sin(RADIANS( 90 + delta_tower_angle_trim[C_AXIS])) * (delta_radius + trt[C_AXIS]);
delta_diagonal_rod_2_tower[A_AXIS] = sq(delta_diagonal_rod + drt[A_AXIS]);
delta_diagonal_rod_2_tower[B_AXIS] = sq(delta_diagonal_rod + drt[B_AXIS]);
delta_diagonal_rod_2_tower[C_AXIS] = sq(delta_diagonal_rod + drt[C_AXIS]);
constexpr abc_float_t trt = DELTA_RADIUS_TRIM_TOWER,
drt = DELTA_DIAGONAL_ROD_TRIM_TOWER;
delta_tower[A_AXIS].set(cos(RADIANS(210 + delta_tower_angle_trim.a)) * (delta_radius + trt.a), // front left tower
sin(RADIANS(210 + delta_tower_angle_trim.a)) * (delta_radius + trt.a));
delta_tower[B_AXIS].set(cos(RADIANS(330 + delta_tower_angle_trim.b)) * (delta_radius + trt.b), // front right tower
sin(RADIANS(330 + delta_tower_angle_trim.b)) * (delta_radius + trt.b));
delta_tower[C_AXIS].set(cos(RADIANS( 90 + delta_tower_angle_trim.c)) * (delta_radius + trt.c), // back middle tower
sin(RADIANS( 90 + delta_tower_angle_trim.c)) * (delta_radius + trt.c));
delta_diagonal_rod_2_tower.set(sq(delta_diagonal_rod + drt.a),
sq(delta_diagonal_rod + drt.b),
sq(delta_diagonal_rod + drt.c));
update_software_endstops(Z_AXIS);
set_all_unhomed();
}
@ -101,18 +100,16 @@ void recalc_delta_settings() {
*/
#define DELTA_DEBUG(VAR) do { \
SERIAL_ECHOLNPAIR("Cartesian X", VAR[X_AXIS], " Y", VAR[Y_AXIS], " Z", VAR[Z_AXIS]); \
SERIAL_ECHOLNPAIR("Delta A", delta[A_AXIS], " B", delta[B_AXIS], " C", delta[C_AXIS]); \
SERIAL_ECHOLNPAIR("Cartesian X", VAR.x, " Y", VAR.y, " Z", VAR.z); \
SERIAL_ECHOLNPAIR("Delta A", delta.a, " B", delta.b, " C", delta.c); \
}while(0)
void inverse_kinematics(const float (&raw)[XYZ]) {
void inverse_kinematics(const xyz_pos_t &raw) {
#if HAS_HOTEND_OFFSET
// Delta hotend offsets must be applied in Cartesian space with no "spoofing"
const float pos[XYZ] = {
raw[X_AXIS] - hotend_offset[X_AXIS][active_extruder],
raw[Y_AXIS] - hotend_offset[Y_AXIS][active_extruder],
raw[Z_AXIS]
};
xyz_pos_t pos = { raw.x - hotend_offset[active_extruder].x,
raw.y - hotend_offset[active_extruder].y,
raw.z };
DELTA_IK(pos);
//DELTA_DEBUG(pos);
#else
@ -126,12 +123,12 @@ void inverse_kinematics(const float (&raw)[XYZ]) {
* effector has the full range of XY motion.
*/
float delta_safe_distance_from_top() {
float cartesian[XYZ] = { 0, 0, 0 };
xyz_pos_t cartesian{0};
inverse_kinematics(cartesian);
float centered_extent = delta[A_AXIS];
cartesian[Y_AXIS] = DELTA_PRINTABLE_RADIUS;
const float centered_extent = delta.a;
cartesian.y = DELTA_PRINTABLE_RADIUS;
inverse_kinematics(cartesian);
return ABS(centered_extent - delta[A_AXIS]);
return ABS(centered_extent - delta.a);
}
/**
@ -161,7 +158,7 @@ float delta_safe_distance_from_top() {
*/
void forward_kinematics_DELTA(const float &z1, const float &z2, const float &z3) {
// Create a vector in old coordinates along x axis of new coordinate
const float p12[3] = { delta_tower[B_AXIS][X_AXIS] - delta_tower[A_AXIS][X_AXIS], delta_tower[B_AXIS][Y_AXIS] - delta_tower[A_AXIS][Y_AXIS], z2 - z1 },
const float p12[3] = { delta_tower[B_AXIS].x - delta_tower[A_AXIS].x, delta_tower[B_AXIS].y - delta_tower[A_AXIS].y, z2 - z1 },
// Get the reciprocal of Magnitude of vector.
d2 = sq(p12[0]) + sq(p12[1]) + sq(p12[2]), inv_d = RSQRT(d2),
@ -170,7 +167,7 @@ void forward_kinematics_DELTA(const float &z1, const float &z2, const float &z3)
ex[3] = { p12[0] * inv_d, p12[1] * inv_d, p12[2] * inv_d },
// Get the vector from the origin of the new system to the third point.
p13[3] = { delta_tower[C_AXIS][X_AXIS] - delta_tower[A_AXIS][X_AXIS], delta_tower[C_AXIS][Y_AXIS] - delta_tower[A_AXIS][Y_AXIS], z3 - z1 },
p13[3] = { delta_tower[C_AXIS].x - delta_tower[A_AXIS].x, delta_tower[C_AXIS].y - delta_tower[A_AXIS].y, z3 - z1 },
// Use the dot product to find the component of this vector on the X axis.
i = ex[0] * p13[0] + ex[1] * p13[1] + ex[2] * p13[2],
@ -198,16 +195,16 @@ void forward_kinematics_DELTA(const float &z1, const float &z2, const float &z3)
// We now have the d, i and j values defined in Wikipedia.
// Plug them into the equations defined in Wikipedia for Xnew, Ynew and Znew
Xnew = (delta_diagonal_rod_2_tower[A_AXIS] - delta_diagonal_rod_2_tower[B_AXIS] + d2) * inv_d * 0.5,
Ynew = ((delta_diagonal_rod_2_tower[A_AXIS] - delta_diagonal_rod_2_tower[C_AXIS] + sq(i) + j2) * 0.5 - i * Xnew) * inv_j,
Znew = SQRT(delta_diagonal_rod_2_tower[A_AXIS] - HYPOT2(Xnew, Ynew));
Xnew = (delta_diagonal_rod_2_tower.a - delta_diagonal_rod_2_tower.b + d2) * inv_d * 0.5,
Ynew = ((delta_diagonal_rod_2_tower.a - delta_diagonal_rod_2_tower.c + sq(i) + j2) * 0.5 - i * Xnew) * inv_j,
Znew = SQRT(delta_diagonal_rod_2_tower.a - HYPOT2(Xnew, Ynew));
// Start from the origin of the old coordinates and add vectors in the
// old coords that represent the Xnew, Ynew and Znew to find the point
// in the old system.
cartes[X_AXIS] = delta_tower[A_AXIS][X_AXIS] + ex[0] * Xnew + ey[0] * Ynew - ez[0] * Znew;
cartes[Y_AXIS] = delta_tower[A_AXIS][Y_AXIS] + ex[1] * Xnew + ey[1] * Ynew - ez[1] * Znew;
cartes[Z_AXIS] = z1 + ex[2] * Xnew + ey[2] * Ynew - ez[2] * Znew;
cartes.set(delta_tower[A_AXIS].x + ex[0] * Xnew + ey[0] * Ynew - ez[0] * Znew,
delta_tower[A_AXIS].y + ex[1] * Xnew + ey[1] * Ynew - ez[1] * Znew,
z1 + ex[2] * Xnew + ey[2] * Ynew - ez[2] * Znew);
}
/**
@ -217,8 +214,8 @@ void forward_kinematics_DELTA(const float &z1, const float &z2, const float &z3)
void home_delta() {
if (DEBUGGING(LEVELING)) DEBUG_POS(">>> home_delta", current_position);
// Init the current position of all carriages to 0,0,0
ZERO(current_position);
ZERO(destination);
current_position.reset();
destination.reset();
sync_plan_position();
// Disable stealthChop if used. Enable diag1 pin on driver.
@ -231,9 +228,9 @@ void home_delta() {
#endif
// Move all carriages together linearly until an endstop is hit.
current_position[Z_AXIS] = (delta_height + 10
current_position.z = (delta_height + 10
#if HAS_BED_PROBE
- probe_offset[Z_AXIS]
- probe_offset.z
#endif
);
line_to_current_position(homing_feedrate(X_AXIS));

41
Marlin/src/module/delta.h
View File

@ -25,17 +25,18 @@
* delta.h - Delta-specific functions
*/
extern float delta_height,
delta_endstop_adj[ABC],
delta_radius,
#include "../core/types.h"
extern float delta_height;
extern abc_float_t delta_endstop_adj;
extern float delta_radius,
delta_diagonal_rod,
delta_segments_per_second,
delta_calibration_radius,
delta_tower_angle_trim[ABC];
extern float delta_tower[ABC][2],
delta_diagonal_rod_2_tower[ABC],
delta_clip_start_height;
delta_calibration_radius;
extern abc_float_t delta_tower_angle_trim;
extern xy_float_t delta_tower[ABC];
extern abc_float_t delta_diagonal_rod_2_tower;
extern float delta_clip_start_height;
/**
* Recalculate factors used for delta kinematics whenever
@ -63,24 +64,16 @@ void recalc_delta_settings();
*/
// Macro to obtain the Z position of an individual tower
#define DELTA_Z(V,T) V[Z_AXIS] + SQRT( \
#define DELTA_Z(V,T) V.z + SQRT( \
delta_diagonal_rod_2_tower[T] - HYPOT2( \
delta_tower[T][X_AXIS] - V[X_AXIS], \
delta_tower[T][Y_AXIS] - V[Y_AXIS] \
delta_tower[T].x - V.x, \
delta_tower[T].y - V.y \
) \
)
#define DELTA_IK(V) do { \
delta[A_AXIS] = DELTA_Z(V, A_AXIS); \
delta[B_AXIS] = DELTA_Z(V, B_AXIS); \
delta[C_AXIS] = DELTA_Z(V, C_AXIS); \
}while(0)
#define DELTA_IK(V) delta.set(DELTA_Z(V, A_AXIS), DELTA_Z(V, B_AXIS), DELTA_Z(V, C_AXIS))
void inverse_kinematics(const float (&raw)[XYZ]);
FORCE_INLINE void inverse_kinematics(const float (&raw)[XYZE]) {
const float raw_xyz[XYZ] = { raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS] };
inverse_kinematics(raw_xyz);
}
void inverse_kinematics(const xyz_pos_t &raw);
/**
* Calculate the highest Z position where the
@ -115,8 +108,8 @@ float delta_safe_distance_from_top();
*/
void forward_kinematics_DELTA(const float &z1, const float &z2, const float &z3);
FORCE_INLINE void forward_kinematics_DELTA(const float (&point)[ABC]) {
forward_kinematics_DELTA(point[A_AXIS], point[B_AXIS], point[C_AXIS]);
FORCE_INLINE void forward_kinematics_DELTA(const abc_float_t &point) {
forward_kinematics_DELTA(point.a, point.b, point.c);
}
void home_delta();

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

99
Marlin/src/module/motion.h
View File

@ -61,15 +61,15 @@ constexpr float slop = 0.0001;
extern bool relative_mode;
extern float current_position[XYZE], // High-level current tool position
destination[XYZE]; // Destination for a move
extern xyze_pos_t current_position, // High-level current tool position
destination; // Destination for a move
// Scratch space for a cartesian result
extern float cartes[XYZ];
extern xyz_pos_t cartes;
// Until kinematics.cpp is created, declare this here
#if IS_KINEMATIC
extern float delta[ABC];
extern abc_pos_t delta;
#endif
#if HAS_ABL_NOT_UBL
@ -81,6 +81,10 @@ extern float cartes[XYZ];
#define XY_PROBE_FEEDRATE_MM_S PLANNER_XY_FEEDRATE()
#endif
#if ENABLED(Z_SAFE_HOMING)
constexpr xy_float_t safe_homing_xy = { Z_SAFE_HOMING_X_POINT, Z_SAFE_HOMING_Y_POINT };
#endif
/**
* Feed rates are often configured with mm/m
* but the planner and stepper like mm/s units.
@ -106,10 +110,10 @@ extern int16_t feedrate_percentage;
FORCE_INLINE float pgm_read_any(const float *p) { return pgm_read_float(p); }
FORCE_INLINE signed char pgm_read_any(const signed char *p) { return pgm_read_byte(p); }
#define XYZ_DEFS(type, array, CONFIG) \
extern const type array##_P[XYZ]; \
FORCE_INLINE type array(AxisEnum axis) { return pgm_read_any(&array##_P[axis]); } \
typedef void __void_##CONFIG##__
#define XYZ_DEFS(T, NAME, OPT) \
extern const XYZval<T> NAME##_P; \
FORCE_INLINE T NAME(AxisEnum axis) { return pgm_read_any(&NAME##_P[axis]); } \
typedef void __void_##OPT##__
XYZ_DEFS(float, base_min_pos, MIN_POS);
XYZ_DEFS(float, base_max_pos, MAX_POS);
@ -125,19 +129,19 @@ XYZ_DEFS(signed char, home_dir, HOME_DIR);
#endif
#if HAS_HOTEND_OFFSET
extern float hotend_offset[XYZ][HOTENDS];
extern xyz_pos_t hotend_offset[HOTENDS];
void reset_hotend_offsets();
#elif HOTENDS > 0
constexpr float hotend_offset[XYZ][HOTENDS] = { { 0 }, { 0 }, { 0 } };
#elif HOTENDS
constexpr xyz_pos_t hotend_offset[HOTENDS] = { { 0 } };
#else
constexpr float hotend_offset[XYZ][1] = { { 0 }, { 0 }, { 0 } };
constexpr xyz_pos_t hotend_offset[1] = { { 0 } };
#endif
typedef struct { float min, max; } axis_limits_t;
typedef struct { xyz_pos_t min, max; } axis_limits_t;
#if HAS_SOFTWARE_ENDSTOPS
extern bool soft_endstops_enabled;
extern axis_limits_t soft_endstop[XYZ];
void apply_motion_limits(float target[XYZ]);
extern axis_limits_t soft_endstop;
void apply_motion_limits(xyz_pos_t &target);
void update_software_endstops(const AxisEnum axis
#if HAS_HOTEND_OFFSET
, const uint8_t old_tool_index=0, const uint8_t new_tool_index=0
@ -145,16 +149,15 @@ typedef struct { float min, max; } axis_limits_t;
);
#else
constexpr bool soft_endstops_enabled = false;
//constexpr axis_limits_t soft_endstop[XYZ] = { { X_MIN_POS, X_MAX_POS }, { Y_MIN_POS, Y_MAX_POS }, { Z_MIN_POS, Z_MAX_POS } };
//constexpr axis_limits_t soft_endstop = {
// { X_MIN_POS, Y_MIN_POS, Z_MIN_POS },
// { X_MAX_POS, Y_MAX_POS, Z_MAX_POS } };
#define apply_motion_limits(V) NOOP
#define update_software_endstops(...) NOOP
#endif
void report_current_position();
inline void set_current_from_destination() { COPY(current_position, destination); }
inline void set_destination_from_current() { COPY(destination, current_position); }
void get_cartesian_from_steppers();
void set_current_from_steppers_for_axis(const AxisEnum axis);
@ -202,12 +205,17 @@ void do_blocking_move_to_y(const float &ry, const feedRate_t &fr_mm_s=0.0f);
void do_blocking_move_to_z(const float &rz, const feedRate_t &fr_mm_s=0.0f);
void do_blocking_move_to_xy(const float &rx, const float &ry, const feedRate_t &fr_mm_s=0.0f);
FORCE_INLINE void do_blocking_move_to(const float (&raw)[XYZ], const feedRate_t &fr_mm_s=0) {
do_blocking_move_to(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], fr_mm_s);
FORCE_INLINE void do_blocking_move_to(const xy_pos_t &raw, const feedRate_t &fr_mm_s=0.0f) {
do_blocking_move_to(raw.x, raw.y, current_position.z, fr_mm_s);
}
FORCE_INLINE void do_blocking_move_to(const float (&raw)[XYZE], const feedRate_t &fr_mm_s=0) {
do_blocking_move_to(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], fr_mm_s);
FORCE_INLINE void do_blocking_move_to(const xyz_pos_t &raw, const feedRate_t &fr_mm_s=0.0f) {
do_blocking_move_to(raw.x, raw.y, raw.z, fr_mm_s);
}
FORCE_INLINE void do_blocking_move_to(const xyze_pos_t &raw, const feedRate_t &fr_mm_s=0.0f) {
do_blocking_move_to(raw.x, raw.y, raw.z, fr_mm_s);
}
FORCE_INLINE void do_blocking_move_to_xy(const xy_pos_t &raw, const feedRate_t &fr_mm_s=0.0f) {
do_blocking_move_to_xy(raw.x, raw.y, fr_mm_s);
}
void remember_feedrate_and_scaling();
@ -238,24 +246,36 @@ void homeaxis(const AxisEnum axis);
*/
#if HAS_HOME_OFFSET || HAS_POSITION_SHIFT
#if HAS_HOME_OFFSET
extern float home_offset[XYZ];
extern xyz_pos_t home_offset;
#endif
#if HAS_POSITION_SHIFT
extern float position_shift[XYZ];
extern xyz_pos_t position_shift;
#endif
#if HAS_HOME_OFFSET && HAS_POSITION_SHIFT
extern float workspace_offset[XYZ];
#define WORKSPACE_OFFSET(AXIS) workspace_offset[AXIS]
extern xyz_pos_t workspace_offset;
#define _WS workspace_offset
#elif HAS_HOME_OFFSET
#define WORKSPACE_OFFSET(AXIS) home_offset[AXIS]
#define _WS home_offset
#else
#define WORKSPACE_OFFSET(AXIS) position_shift[AXIS]
#define _WS position_shift
#endif
#define NATIVE_TO_LOGICAL(POS, AXIS) ((POS) + WORKSPACE_OFFSET(AXIS))
#define LOGICAL_TO_NATIVE(POS, AXIS) ((POS) - WORKSPACE_OFFSET(AXIS))
#define NATIVE_TO_LOGICAL(POS, AXIS) ((POS) + _WS[AXIS])
#define LOGICAL_TO_NATIVE(POS, AXIS) ((POS) - _WS[AXIS])
FORCE_INLINE void toLogical(xy_pos_t &raw) { raw += _WS; }
FORCE_INLINE void toLogical(xyz_pos_t &raw) { raw += _WS; }
FORCE_INLINE void toLogical(xyze_pos_t &raw) { raw += _WS; }
FORCE_INLINE void toNative(xy_pos_t &raw) { raw -= _WS; }
FORCE_INLINE void toNative(xyz_pos_t &raw) { raw -= _WS; }
FORCE_INLINE void toNative(xyze_pos_t &raw) { raw -= _WS; }
#else
#define NATIVE_TO_LOGICAL(POS, AXIS) (POS)
#define LOGICAL_TO_NATIVE(POS, AXIS) (POS)
FORCE_INLINE void toLogical(xy_pos_t &raw) { UNUSED(raw); }
FORCE_INLINE void toLogical(xyz_pos_t &raw) { UNUSED(raw); }
FORCE_INLINE void toLogical(xyze_pos_t &raw) { UNUSED(raw); }
FORCE_INLINE void toNative(xy_pos_t &raw) { UNUSED(raw); }
FORCE_INLINE void toNative(xyz_pos_t &raw) { UNUSED(raw); }
FORCE_INLINE void toNative(xyze_pos_t &raw) { UNUSED(raw); }
#endif
#define LOGICAL_X_POSITION(POS) NATIVE_TO_LOGICAL(POS, X_AXIS)
#define LOGICAL_Y_POSITION(POS) NATIVE_TO_LOGICAL(POS, Y_AXIS)
@ -270,7 +290,7 @@ void homeaxis(const AxisEnum axis);
#if IS_KINEMATIC // (DELTA or SCARA)
#if HAS_SCARA_OFFSET
extern float scara_home_offset[ABC]; // A and B angular offsets, Z mm offset
extern abc_pos_t scara_home_offset; // A and B angular offsets, Z mm offset
#endif
// Return true if the given point is within the printable area
@ -288,11 +308,15 @@ void homeaxis(const AxisEnum axis);
#endif
}
inline bool position_is_reachable(const xy_pos_t &pos, const float inset=0) {
return position_is_reachable(pos.x, pos.y, inset);
}
#if HAS_BED_PROBE
// Return true if the both nozzle and the probe can reach the given point.
// Note: This won't work on SCARA since the probe offset rotates with the arm.
inline bool position_is_reachable_by_probe(const float &rx, const float &ry) {
return position_is_reachable(rx - probe_offset[X_AXIS], ry - probe_offset[Y_AXIS])
return position_is_reachable(rx - probe_offset.x, ry - probe_offset.y)
&& position_is_reachable(rx, ry, ABS(MIN_PROBE_EDGE));
}
#endif
@ -311,6 +335,7 @@ void homeaxis(const AxisEnum axis);
return WITHIN(rx, X_MIN_POS - slop, X_MAX_POS + slop);
#endif
}
inline bool position_is_reachable(const xy_pos_t &pos) { return position_is_reachable(pos.x, pos.y); }
#if HAS_BED_PROBE
/**
@ -321,7 +346,7 @@ void homeaxis(const AxisEnum axis);
* nozzle must be be able to reach +10,-10.
*/
inline bool position_is_reachable_by_probe(const float &rx, const float &ry) {
return position_is_reachable(rx - probe_offset[X_AXIS], ry - probe_offset[Y_AXIS])
return position_is_reachable(rx - probe_offset.x, ry - probe_offset.y)
&& WITHIN(rx, probe_min_x() - slop, probe_max_x() + slop)
&& WITHIN(ry, probe_min_y() - slop, probe_max_y() + slop);
}
@ -332,6 +357,8 @@ void homeaxis(const AxisEnum axis);
#if !HAS_BED_PROBE
FORCE_INLINE bool position_is_reachable_by_probe(const float &rx, const float &ry) { return position_is_reachable(rx, ry); }
#endif
FORCE_INLINE bool position_is_reachable_by_probe(const xy_int_t &pos) { return position_is_reachable_by_probe(pos.x, pos.y); }
FORCE_INLINE bool position_is_reachable_by_probe(const xy_pos_t &pos) { return position_is_reachable_by_probe(pos.x, pos.y); }
/**
* Duplication mode
@ -358,8 +385,8 @@ void homeaxis(const AxisEnum axis);
extern DualXMode dual_x_carriage_mode;
extern float inactive_extruder_x_pos, // Used in mode 0 & 1
raised_parked_position[XYZE], // Used in mode 1
duplicate_extruder_x_offset; // Used in mode 2 & 3
extern xyz_pos_t raised_parked_position; // Used in mode 1
extern bool active_extruder_parked; // Used in mode 1, 2 & 3
extern millis_t delayed_move_time; // Used in mode 1
extern int16_t duplicate_extruder_temp_offset; // Used in mode 2 & 3

432
Marlin/src/module/planner.cpp
View File

@ -137,9 +137,9 @@ float Planner::steps_to_mm[XYZE_N]; // (mm) Millimeters per step
#endif
#if HAS_CLASSIC_JERK
#if BOTH(JUNCTION_DEVIATION, LIN_ADVANCE)
float Planner::max_jerk[XYZ]; // (mm/s^2) M205 XYZ - The largest speed change requiring no acceleration.
xyz_pos_t Planner::max_jerk; // (mm/s^2) M205 XYZ - The largest speed change requiring no acceleration.
#else
float Planner::max_jerk[XYZE]; // (mm/s^2) M205 XYZE - The largest speed change requiring no acceleration.
xyze_pos_t Planner::max_jerk; // (mm/s^2) M205 XYZE - The largest speed change requiring no acceleration.
#endif
#endif
@ -187,12 +187,12 @@ skew_factor_t Planner::skew_factor; // Initialized by settings.load()
// private:
int32_t Planner::position[NUM_AXIS] = { 0 };
xyze_long_t Planner::position{0};
uint32_t Planner::cutoff_long;
float Planner::previous_speed[NUM_AXIS],
Planner::previous_nominal_speed_sqr;
xyze_float_t Planner::previous_speed;
float Planner::previous_nominal_speed_sqr;
#if ENABLED(DISABLE_INACTIVE_EXTRUDER)
uint8_t Planner::g_uc_extruder_last_move[EXTRUDERS] = { 0 };
@ -202,7 +202,7 @@ float Planner::previous_speed[NUM_AXIS],
// Old direction bits. Used for speed calculations
unsigned char Planner::old_direction_bits = 0;
// Segment times (in µs). Used for speed calculations
uint32_t Planner::axis_segment_time_us[2][3] = { { MAX_FREQ_TIME_US + 1, 0, 0 }, { MAX_FREQ_TIME_US + 1, 0, 0 } };
xy_ulong_t Planner::axis_segment_time_us[3] = { { MAX_FREQ_TIME_US + 1, MAX_FREQ_TIME_US + 1 } };
#endif
#if ENABLED(LIN_ADVANCE)
@ -210,11 +210,11 @@ float Planner::previous_speed[NUM_AXIS],
#endif
#if HAS_POSITION_FLOAT
float Planner::position_float[XYZE]; // Needed for accurate maths. Steps cannot be used!
xyze_pos_t Planner::position_float; // Needed for accurate maths. Steps cannot be used!
#endif
#if IS_KINEMATIC
float Planner::position_cart[XYZE];
xyze_pos_t Planner::position_cart;
#endif
#if HAS_SPI_LCD
@ -228,14 +228,14 @@ float Planner::previous_speed[NUM_AXIS],
Planner::Planner() { init(); }
void Planner::init() {
ZERO(position);
position.reset();
#if HAS_POSITION_FLOAT
ZERO(position_float);
position_float.reset();
#endif
#if IS_KINEMATIC
ZERO(position_cart);
position_cart.reset();
#endif
ZERO(previous_speed);
previous_speed.reset();
previous_nominal_speed_sqr = 0;
#if ABL_PLANAR
bed_level_matrix.set_to_identity();
@ -1155,8 +1155,8 @@ void Planner::recalculate() {
float high = 0.0;
for (uint8_t b = block_buffer_tail; b != block_buffer_head; b = next_block_index(b)) {
block_t* block = &block_buffer[b];
if (block->steps[X_AXIS] || block->steps[Y_AXIS] || block->steps[Z_AXIS]) {
const float se = (float)block->steps[E_AXIS] / block->step_event_count * SQRT(block->nominal_speed_sqr); // mm/sec;
if (block->steps.x || block->steps.y || block->steps.z) {
const float se = (float)block->steps.e / block->step_event_count * SQRT(block->nominal_speed_sqr); // mm/sec;
NOLESS(high, se);
}
}
@ -1176,7 +1176,7 @@ void Planner::recalculate() {
void Planner::check_axes_activity() {
#if ANY(DISABLE_X, DISABLE_Y, DISABLE_Z, DISABLE_E)
uint8_t axis_active[NUM_AXIS] = { 0 };
xyze_bool_t axis_active = { false };
#endif
#if FAN_COUNT > 0
@ -1236,16 +1236,16 @@ void Planner::check_axes_activity() {
}
#if ENABLED(DISABLE_X)
if (!axis_active[X_AXIS]) disable_X();
if (!axis_active.x) disable_X();
#endif
#if ENABLED(DISABLE_Y)
if (!axis_active[Y_AXIS]) disable_Y();
if (!axis_active.y) disable_Y();
#endif
#if ENABLED(DISABLE_Z)
if (!axis_active[Z_AXIS]) disable_Z();
if (!axis_active.z) disable_Z();
#endif
#if ENABLED(DISABLE_E)
if (!axis_active[E_AXIS]) disable_e_steppers();
if (!axis_active.e) disable_e_steppers();
#endif
#if FAN_COUNT > 0
@ -1354,40 +1354,32 @@ void Planner::check_axes_activity() {
* rx, ry, rz - Cartesian positions in mm
* Leveled XYZ on completion
*/
void Planner::apply_leveling(float &rx, float &ry, float &rz) {
void Planner::apply_leveling(xyz_pos_t &raw) {
if (!leveling_active) return;
#if ABL_PLANAR
float dx = rx - (X_TILT_FULCRUM),
dy = ry - (Y_TILT_FULCRUM);
apply_rotation_xyz(bed_level_matrix, dx, dy, rz);
rx = dx + X_TILT_FULCRUM;
ry = dy + Y_TILT_FULCRUM;
xy_pos_t d = raw - level_fulcrum;
apply_rotation_xyz(bed_level_matrix, d.x, d.y, raw.z);
raw = d + level_fulcrum;
#elif HAS_MESH
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
const float fade_scaling_factor = fade_scaling_factor_for_z(rz);
const float fade_scaling_factor = fade_scaling_factor_for_z(raw.z);
#else
constexpr float fade_scaling_factor = 1.0;
#endif
#if ENABLED(AUTO_BED_LEVELING_BILINEAR)
const float raw[XYZ] = { rx, ry, 0 };
#endif
rz += (
raw.z += (
#if ENABLED(MESH_BED_LEVELING)
mbl.get_z(rx, ry
mbl.get_z(raw
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
, fade_scaling_factor
#endif
)
#elif ENABLED(AUTO_BED_LEVELING_UBL)
fade_scaling_factor ? fade_scaling_factor * ubl.get_z_correction(rx, ry) : 0.0
fade_scaling_factor ? fade_scaling_factor * ubl.get_z_correction(raw) : 0.0
#elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
fade_scaling_factor ? fade_scaling_factor * bilinear_z_offset(raw) : 0.0
#endif
@ -1396,7 +1388,7 @@ void Planner::check_axes_activity() {
#endif
}
void Planner::unapply_leveling(float raw[XYZ]) {
void Planner::unapply_leveling(xyz_pos_t &raw) {
if (leveling_active) {
@ -1404,31 +1396,27 @@ void Planner::check_axes_activity() {
matrix_3x3 inverse = matrix_3x3::transpose(bed_level_matrix);
float dx = raw[X_AXIS] - (X_TILT_FULCRUM),
dy = raw[Y_AXIS] - (Y_TILT_FULCRUM);
apply_rotation_xyz(inverse, dx, dy, raw[Z_AXIS]);
raw[X_AXIS] = dx + X_TILT_FULCRUM;
raw[Y_AXIS] = dy + Y_TILT_FULCRUM;
xy_pos_t d = raw - level_fulcrum;
apply_rotation_xyz(inverse, d.x, d.y, raw.z);
raw = d + level_fulcrum;
#elif HAS_MESH
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
const float fade_scaling_factor = fade_scaling_factor_for_z(raw[Z_AXIS]);
const float fade_scaling_factor = fade_scaling_factor_for_z(raw.z);
#else
constexpr float fade_scaling_factor = 1.0;
#endif
raw[Z_AXIS] -= (
raw.z -= (
#if ENABLED(MESH_BED_LEVELING)
mbl.get_z(raw[X_AXIS], raw[Y_AXIS]
mbl.get_z(raw
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
, fade_scaling_factor
#endif
)
#elif ENABLED(AUTO_BED_LEVELING_UBL)
fade_scaling_factor ? fade_scaling_factor * ubl.get_z_correction(raw[X_AXIS], raw[Y_AXIS]) : 0.0
fade_scaling_factor ? fade_scaling_factor * ubl.get_z_correction(raw) : 0.0
#elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
fade_scaling_factor ? fade_scaling_factor * bilinear_z_offset(raw) : 0.0
#endif
@ -1438,7 +1426,7 @@ void Planner::check_axes_activity() {
}
#if ENABLED(SKEW_CORRECTION)
unskew(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]);
unskew(raw);
#endif
}
@ -1563,12 +1551,12 @@ void Planner::synchronize() {
*
* Returns true if movement was properly queued, false otherwise
*/
bool Planner::_buffer_steps(const int32_t (&target)[XYZE]
bool Planner::_buffer_steps(const xyze_long_t &target
#if HAS_POSITION_FLOAT
, const float (&target_float)[ABCE]
, const xyze_pos_t &target_float
#endif
#if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION)
, const float (&delta_mm_cart)[XYZE]
, const xyze_float_t &delta_mm_cart
#endif
, feedRate_t fr_mm_s, const uint8_t extruder, const float &millimeters
) {
@ -1627,33 +1615,33 @@ bool Planner::_buffer_steps(const int32_t (&target)[XYZE]
* Returns true is movement is acceptable, false otherwise
*/
bool Planner::_populate_block(block_t * const block, bool split_move,
const int32_t (&target)[ABCE]
const abce_long_t &target
#if HAS_POSITION_FLOAT
, const float (&target_float)[ABCE]
, const xyze_pos_t &target_float
#endif
#if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION)
, const float (&delta_mm_cart)[XYZE]
, const xyze_float_t &delta_mm_cart
#endif
, feedRate_t fr_mm_s, const uint8_t extruder, const float &millimeters/*=0.0*/
) {
const int32_t da = target[A_AXIS] - position[A_AXIS],
db = target[B_AXIS] - position[B_AXIS],
dc = target[C_AXIS] - position[C_AXIS];
const int32_t da = target.a - position.a,
db = target.b - position.b,
dc = target.c - position.c;
#if EXTRUDERS
int32_t de = target[E_AXIS] - position[E_AXIS];
int32_t de = target.e - position.e;
#else
constexpr int32_t de = 0;
#endif
/* <-- add a slash to enable
SERIAL_ECHOLNPAIR(" _populate_block FR:", fr_mm_s,
" A:", target[A_AXIS], " (", da, " steps)"
" B:", target[B_AXIS], " (", db, " steps)"
" C:", target[C_AXIS], " (", dc, " steps)"
" A:", target.a, " (", da, " steps)"
" B:", target.b, " (", db, " steps)"
" C:", target.c, " (", dc, " steps)"
#if EXTRUDERS
" E:", target[E_AXIS], " (", de, " steps)"
" E:", target.e, " (", de, " steps)"
#endif
);
//*/
@ -1662,9 +1650,9 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
if (de) {
#if ENABLED(PREVENT_COLD_EXTRUSION)
if (thermalManager.tooColdToExtrude(extruder)) {
position[E_AXIS] = target[E_AXIS]; // Behave as if the move really took place, but ignore E part
position.e = target.e; // Behave as if the move really took place, but ignore E part
#if HAS_POSITION_FLOAT
position_float[E_AXIS] = target_float[E_AXIS];
position_float.e = target_float.e;
#endif
de = 0; // no difference
SERIAL_ECHO_MSG(MSG_ERR_COLD_EXTRUDE_STOP);
@ -1684,9 +1672,9 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
constexpr bool ignore_e = true;
#endif
if (ignore_e) {
position[E_AXIS] = target[E_AXIS]; // Behave as if the move really took place, but ignore E part
position.e = target.e; // Behave as if the move really took place, but ignore E part
#if HAS_POSITION_FLOAT
position_float[E_AXIS] = target_float[E_AXIS];
position_float.e = target_float.e;
#endif
de = 0; // no difference
SERIAL_ECHO_MSG(MSG_ERR_LONG_EXTRUDE_STOP);
@ -1739,26 +1727,16 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
// Number of steps for each axis
// See http://www.corexy.com/theory.html
#if CORE_IS_XY
block->steps[A_AXIS] = ABS(da + db);
block->steps[B_AXIS] = ABS(da - db);
block->steps[Z_AXIS] = ABS(dc);
block->steps.set(ABS(da + db), ABS(da - db), ABS(dc));
#elif CORE_IS_XZ
block->steps[A_AXIS] = ABS(da + dc);
block->steps[Y_AXIS] = ABS(db);
block->steps[C_AXIS] = ABS(da - dc);
block->steps.set(ABS(da + dc), ABS(db), ABS(da - dc));
#elif CORE_IS_YZ
block->steps[X_AXIS] = ABS(da);
block->steps[B_AXIS] = ABS(db + dc);
block->steps[C_AXIS] = ABS(db - dc);
block->steps.set(ABS(da), ABS(db + dc), ABS(db - dc));
#elif IS_SCARA
block->steps[A_AXIS] = ABS(da);
block->steps[B_AXIS] = ABS(db);
block->steps[Z_AXIS] = ABS(dc);
block->steps.set(ABS(da), ABS(db), ABS(dc));
#else
// default non-h-bot planning
block->steps[A_AXIS] = ABS(da);
block->steps[B_AXIS] = ABS(db);
block->steps[C_AXIS] = ABS(dc);
block->steps.set(ABS(da), ABS(db), ABS(dc));
#endif
/**
@ -1769,42 +1747,45 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
* So we need to create other 2 "AXIS", named X_HEAD and Y_HEAD, meaning the real displacement of the Head.
* Having the real displacement of the head, we can calculate the total movement length and apply the desired speed.
*/
struct DeltaMM : abce_float_t {
#if IS_CORE
xyz_pos_t head;
#endif
} delta_mm;
#if IS_CORE
float delta_mm[Z_HEAD + 1];
#if CORE_IS_XY
delta_mm[X_HEAD] = da * steps_to_mm[A_AXIS];
delta_mm[Y_HEAD] = db * steps_to_mm[B_AXIS];
delta_mm[Z_AXIS] = dc * steps_to_mm[Z_AXIS];
delta_mm[A_AXIS] = (da + db) * steps_to_mm[A_AXIS];
delta_mm[B_AXIS] = CORESIGN(da - db) * steps_to_mm[B_AXIS];
delta_mm.head.x = da * steps_to_mm[A_AXIS];
delta_mm.head.y = db * steps_to_mm[B_AXIS];
delta_mm.z = dc * steps_to_mm[Z_AXIS];
delta_mm.a = (da + db) * steps_to_mm[A_AXIS];
delta_mm.b = CORESIGN(da - db) * steps_to_mm[B_AXIS];
#elif CORE_IS_XZ
delta_mm[X_HEAD] = da * steps_to_mm[A_AXIS];
delta_mm[Y_AXIS] = db * steps_to_mm[Y_AXIS];
delta_mm[Z_HEAD] = dc * steps_to_mm[C_AXIS];
delta_mm[A_AXIS] = (da + dc) * steps_to_mm[A_AXIS];
delta_mm[C_AXIS] = CORESIGN(da - dc) * steps_to_mm[C_AXIS];
delta_mm.head.x = da * steps_to_mm[A_AXIS];
delta_mm.y = db * steps_to_mm[Y_AXIS];
delta_mm.head.z = dc * steps_to_mm[C_AXIS];
delta_mm.a = (da + dc) * steps_to_mm[A_AXIS];
delta_mm.c = CORESIGN(da - dc) * steps_to_mm[C_AXIS];
#elif CORE_IS_YZ
delta_mm[X_AXIS] = da * steps_to_mm[X_AXIS];
delta_mm[Y_HEAD] = db * steps_to_mm[B_AXIS];
delta_mm[Z_HEAD] = dc * steps_to_mm[C_AXIS];
delta_mm[B_AXIS] = (db + dc) * steps_to_mm[B_AXIS];
delta_mm[C_AXIS] = CORESIGN(db - dc) * steps_to_mm[C_AXIS];
delta_mm.x = da * steps_to_mm[X_AXIS];
delta_mm.head.y = db * steps_to_mm[B_AXIS];
delta_mm.head.z = dc * steps_to_mm[C_AXIS];
delta_mm.b = (db + dc) * steps_to_mm[B_AXIS];
delta_mm.c = CORESIGN(db - dc) * steps_to_mm[C_AXIS];
#endif
#else
float delta_mm[ABCE];
delta_mm[A_AXIS] = da * steps_to_mm[A_AXIS];
delta_mm[B_AXIS] = db * steps_to_mm[B_AXIS];
delta_mm[C_AXIS] = dc * steps_to_mm[C_AXIS];
delta_mm.a = da * steps_to_mm[A_AXIS];
delta_mm.b = db * steps_to_mm[B_AXIS];
delta_mm.c = dc * steps_to_mm[C_AXIS];
#endif
#if EXTRUDERS
delta_mm[E_AXIS] = esteps_float * steps_to_mm[E_AXIS_N(extruder)];
delta_mm.e = esteps_float * steps_to_mm[E_AXIS_N(extruder)];
#endif
if (block->steps[A_AXIS] < MIN_STEPS_PER_SEGMENT && block->steps[B_AXIS] < MIN_STEPS_PER_SEGMENT && block->steps[C_AXIS] < MIN_STEPS_PER_SEGMENT) {
if (block->steps.a < MIN_STEPS_PER_SEGMENT && block->steps.b < MIN_STEPS_PER_SEGMENT && block->steps.c < MIN_STEPS_PER_SEGMENT) {
block->millimeters = (0
#if EXTRUDERS
+ ABS(delta_mm[E_AXIS])
+ ABS(delta_mm.e)
#endif
);
}
@ -1814,13 +1795,13 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
else
block->millimeters = SQRT(
#if CORE_IS_XY
sq(delta_mm[X_HEAD]) + sq(delta_mm[Y_HEAD]) + sq(delta_mm[Z_AXIS])
sq(delta_mm.head.x) + sq(delta_mm.head.y) + sq(delta_mm.z)
#elif CORE_IS_XZ
sq(delta_mm[X_HEAD]) + sq(delta_mm[Y_AXIS]) + sq(delta_mm[Z_HEAD])
sq(delta_mm.head.x) + sq(delta_mm.y) + sq(delta_mm.head.z)
#elif CORE_IS_YZ
sq(delta_mm[X_AXIS]) + sq(delta_mm[Y_HEAD]) + sq(delta_mm[Z_HEAD])
sq(delta_mm.x) + sq(delta_mm.head.y) + sq(delta_mm.head.z)
#else
sq(delta_mm[X_AXIS]) + sq(delta_mm[Y_AXIS]) + sq(delta_mm[Z_AXIS])
sq(delta_mm.x) + sq(delta_mm.y) + sq(delta_mm.z)
#endif
);
@ -1839,10 +1820,10 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
}
#if EXTRUDERS
block->steps[E_AXIS] = esteps;
block->steps.e = esteps;
#endif
block->step_event_count = _MAX(block->steps[A_AXIS], block->steps[B_AXIS], block->steps[C_AXIS], esteps);
block->step_event_count = _MAX(block->steps.a, block->steps.b, block->steps.c, esteps);
// Bail if this is a zero-length block
if (block->step_event_count < MIN_STEPS_PER_SEGMENT) return false;
@ -1865,36 +1846,36 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
#endif
#if ENABLED(AUTO_POWER_CONTROL)
if (block->steps[X_AXIS] || block->steps[Y_AXIS] || block->steps[Z_AXIS])
if (block->steps.x || block->steps.y || block->steps.z)
powerManager.power_on();
#endif
// Enable active axes
#if CORE_IS_XY
if (block->steps[A_AXIS] || block->steps[B_AXIS]) {
if (block->steps.a || block->steps.b) {
enable_X();
enable_Y();
}
#if DISABLED(Z_LATE_ENABLE)
if (block->steps[Z_AXIS]) enable_Z();
if (block->steps.z) enable_Z();
#endif
#elif CORE_IS_XZ
if (block->steps[A_AXIS] || block->steps[C_AXIS]) {
if (block->steps.a || block->steps.c) {
enable_X();
enable_Z();
}
if (block->steps[Y_AXIS]) enable_Y();
if (block->steps.y) enable_Y();
#elif CORE_IS_YZ
if (block->steps[B_AXIS] || block->steps[C_AXIS]) {
if (block->steps.b || block->steps.c) {
enable_Y();
enable_Z();
}
if (block->steps[X_AXIS]) enable_X();
if (block->steps.x) enable_X();
#else
if (block->steps[X_AXIS]) enable_X();
if (block->steps[Y_AXIS]) enable_Y();
if (block->steps.x) enable_X();
if (block->steps.y) enable_Y();
#if DISABLED(Z_LATE_ENABLE)
if (block->steps[Z_AXIS]) enable_Z();
if (block->steps.z) enable_Z();
#endif
#endif
@ -2074,20 +2055,21 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
#if ENABLED(FILAMENT_WIDTH_SENSOR)
if (extruder == FILAMENT_SENSOR_EXTRUDER_NUM) // Only for extruder with filament sensor
filwidth.advance_e(delta_mm[E_AXIS]);
filwidth.advance_e(delta_mm.e);
#endif
// Calculate and limit speed in mm/sec for each axis
float current_speed[NUM_AXIS], speed_factor = 1.0f; // factor <1 decreases speed
xyze_float_t current_speed;
float speed_factor = 1.0f; // factor <1 decreases speed
LOOP_XYZE(i) {
#if BOTH(MIXING_EXTRUDER, RETRACT_SYNC_MIXING)
// In worst case, only one extruder running, no change is needed.
// In best case, all extruders run the same amount, we can divide by MIXING_STEPPERS
float delta_mm_i = 0;
if (i == E_AXIS && mixer.get_current_vtool() == MIXER_AUTORETRACT_TOOL)
delta_mm_i = delta_mm[i] / MIXING_STEPPERS;
delta_mm_i = delta_mm.e / MIXING_STEPPERS;
else
delta_mm_i = delta_mm[i];
delta_mm_i = delta_mm.e;
#else
const float delta_mm_i = delta_mm[i];
#endif
@ -2106,26 +2088,26 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
old_direction_bits = block->direction_bits;
segment_time_us = LROUND((float)segment_time_us / speed_factor);
uint32_t xs0 = axis_segment_time_us[X_AXIS][0],
xs1 = axis_segment_time_us[X_AXIS][1],
xs2 = axis_segment_time_us[X_AXIS][2],
ys0 = axis_segment_time_us[Y_AXIS][0],
ys1 = axis_segment_time_us[Y_AXIS][1],
ys2 = axis_segment_time_us[Y_AXIS][2];
uint32_t xs0 = axis_segment_time_us[0].x,
xs1 = axis_segment_time_us[1].x,
xs2 = axis_segment_time_us[2].x,
ys0 = axis_segment_time_us[0].y,
ys1 = axis_segment_time_us[1].y,
ys2 = axis_segment_time_us[2].y;
if (TEST(direction_change, X_AXIS)) {
xs2 = axis_segment_time_us[X_AXIS][2] = xs1;
xs1 = axis_segment_time_us[X_AXIS][1] = xs0;
xs2 = axis_segment_time_us[2].x = xs1;
xs1 = axis_segment_time_us[1].x = xs0;
xs0 = 0;
}
xs0 = axis_segment_time_us[X_AXIS][0] = xs0 + segment_time_us;
xs0 = axis_segment_time_us[0].x = xs0 + segment_time_us;
if (TEST(direction_change, Y_AXIS)) {
ys2 = axis_segment_time_us[Y_AXIS][2] = axis_segment_time_us[Y_AXIS][1];
ys1 = axis_segment_time_us[Y_AXIS][1] = axis_segment_time_us[Y_AXIS][0];
ys2 = axis_segment_time_us[2].y = axis_segment_time_us[1].y;
ys1 = axis_segment_time_us[1].y = axis_segment_time_us[0].y;
ys0 = 0;
}
ys0 = axis_segment_time_us[Y_AXIS][0] = ys0 + segment_time_us;
ys0 = axis_segment_time_us[0].y = ys0 + segment_time_us;
const uint32_t max_x_segment_time = _MAX(xs0, xs1, xs2),
max_y_segment_time = _MAX(ys0, ys1, ys2),
@ -2138,7 +2120,7 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
// Correct the speed
if (speed_factor < 1.0f) {
LOOP_XYZE(i) current_speed[i] *= speed_factor;
current_speed *= speed_factor;
block->nominal_rate *= speed_factor;
block->nominal_speed_sqr = block->nominal_speed_sqr * sq(speed_factor);
}
@ -2146,7 +2128,7 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
// Compute and limit the acceleration rate for the trapezoid generator.
const float steps_per_mm = block->step_event_count * inverse_millimeters;
uint32_t accel;
if (!block->steps[A_AXIS] && !block->steps[B_AXIS] && !block->steps[C_AXIS]) {
if (!block->steps.a && !block->steps.b && !block->steps.c) {
// convert to: acceleration steps/sec^2
accel = CEIL(settings.retract_acceleration * steps_per_mm);
#if ENABLED(LIN_ADVANCE)
@ -2180,7 +2162,7 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
#define MAX_E_JERK max_e_jerk
#endif
#else
#define MAX_E_JERK max_jerk[E_AXIS]
#define MAX_E_JERK max_jerk.e
#endif
/**
@ -2198,13 +2180,13 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
&& de > 0;
if (block->use_advance_lead) {
block->e_D_ratio = (target_float[E_AXIS] - position_float[E_AXIS]) /
block->e_D_ratio = (target_float.e - position_float.e) /
#if IS_KINEMATIC
block->millimeters
#else
SQRT(sq(target_float[X_AXIS] - position_float[X_AXIS])
+ sq(target_float[Y_AXIS] - position_float[Y_AXIS])
+ sq(target_float[Z_AXIS] - position_float[Z_AXIS]))
SQRT(sq(target_float.x - position_float.x)
+ sq(target_float.y - position_float.y)
+ sq(target_float.z - position_float.z))
#endif
;
@ -2297,23 +2279,16 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
already calculated in a different place. */
// Unit vector of previous path line segment
static float previous_unit_vec[XYZE];
static xyze_float_t prev_unit_vec;
#if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION)
float unit_vec[] = {
delta_mm_cart[X_AXIS] * inverse_millimeters,
delta_mm_cart[Y_AXIS] * inverse_millimeters,
delta_mm_cart[Z_AXIS] * inverse_millimeters,
delta_mm_cart[E_AXIS] * inverse_millimeters
};
#else
float unit_vec[] = {
delta_mm[X_AXIS] * inverse_millimeters,
delta_mm[Y_AXIS] * inverse_millimeters,
delta_mm[Z_AXIS] * inverse_millimeters,
delta_mm[E_AXIS] * inverse_millimeters
};
#endif
xyze_float_t unit_vec =
#if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION)
delta_mm_cart
#else
{ delta_mm.x, delta_mm.y, delta_mm.z, delta_mm.e }
#endif
;
unit_vec *= inverse_millimeters;
#if IS_CORE && ENABLED(JUNCTION_DEVIATION)
/**
@ -2328,11 +2303,8 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
if (moves_queued && !UNEAR_ZERO(previous_nominal_speed_sqr)) {
// Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
// NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
float junction_cos_theta = -previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
-previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
-previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS]
-previous_unit_vec[E_AXIS] * unit_vec[E_AXIS]
;
float junction_cos_theta = (-prev_unit_vec.x * unit_vec.x) + (-prev_unit_vec.y * unit_vec.y)
+ (-prev_unit_vec.z * unit_vec.z) + (-prev_unit_vec.e * unit_vec.e);
// NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta).
if (junction_cos_theta > 0.999999f) {
@ -2343,12 +2315,7 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
NOLESS(junction_cos_theta, -0.999999f); // Check for numerical round-off to avoid divide by zero.
// Convert delta vector to unit vector
float junction_unit_vec[XYZE] = {
unit_vec[X_AXIS] - previous_unit_vec[X_AXIS],
unit_vec[Y_AXIS] - previous_unit_vec[Y_AXIS],
unit_vec[Z_AXIS] - previous_unit_vec[Z_AXIS],
unit_vec[E_AXIS] - previous_unit_vec[E_AXIS]
};
xyze_float_t junction_unit_vec = unit_vec - prev_unit_vec;
normalize_junction_vector(junction_unit_vec);
const float junction_acceleration = limit_value_by_axis_maximum(block->acceleration, junction_unit_vec),
@ -2374,7 +2341,7 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
else // Init entry speed to zero. Assume it starts from rest. Planner will correct this later.
vmax_junction_sqr = 0;
COPY(previous_unit_vec, unit_vec);
prev_unit_vec = unit_vec;
#endif
@ -2497,18 +2464,17 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
block->flag |= block->nominal_speed_sqr <= v_allowable_sqr ? BLOCK_FLAG_RECALCULATE | BLOCK_FLAG_NOMINAL_LENGTH : BLOCK_FLAG_RECALCULATE;
// Update previous path unit_vector and nominal speed
COPY(previous_speed, current_speed);
previous_speed = current_speed;
previous_nominal_speed_sqr = block->nominal_speed_sqr;
// Update the position
static_assert(COUNT(target) > 1, "Parameter to _buffer_steps must be (&target)[XYZE]!");
COPY(position, target);
position = target;
#if HAS_POSITION_FLOAT
COPY(position_float, target_float);
position_float = target_float;
#endif
#if ENABLED(GRADIENT_MIX)
mixer.gradient_control(target_float[Z_AXIS]);
mixer.gradient_control(target_float.z);
#endif
#if ENABLED(POWER_LOSS_RECOVERY)
@ -2533,10 +2499,7 @@ void Planner::buffer_sync_block() {
block->flag = BLOCK_FLAG_SYNC_POSITION;
block->position[A_AXIS] = position[A_AXIS];
block->position[B_AXIS] = position[B_AXIS];
block->position[C_AXIS] = position[C_AXIS];
block->position[E_AXIS] = position[E_AXIS];
block->position = position;
// If this is the first added movement, reload the delay, otherwise, cancel it.
if (block_buffer_head == block_buffer_tail) {
@ -2567,7 +2530,7 @@ void Planner::buffer_sync_block() {
*/
bool Planner::buffer_segment(const float &a, const float &b, const float &c, const float &e
#if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION)
, const float (&delta_mm_cart)[XYZE]
, const xyze_float_t &delta_mm_cart
#endif
, const feedRate_t &fr_mm_s, const uint8_t extruder, const float &millimeters/*=0.0*/
) {
@ -2578,14 +2541,14 @@ bool Planner::buffer_segment(const float &a, const float &b, const float &c, con
// When changing extruders recalculate steps corresponding to the E position
#if ENABLED(DISTINCT_E_FACTORS)
if (last_extruder != extruder && settings.axis_steps_per_mm[E_AXIS_N(extruder)] != settings.axis_steps_per_mm[E_AXIS_N(last_extruder)]) {
position[E_AXIS] = LROUND(position[E_AXIS] * settings.axis_steps_per_mm[E_AXIS_N(extruder)] * steps_to_mm[E_AXIS_N(last_extruder)]);
position.e = LROUND(position.e * settings.axis_steps_per_mm[E_AXIS_N(extruder)] * steps_to_mm[E_AXIS_N(last_extruder)]);
last_extruder = extruder;
}
#endif
// The target position of the tool in absolute steps
// Calculate target position in absolute steps
const int32_t target[ABCE] = {
const abce_long_t target = {
int32_t(LROUND(a * settings.axis_steps_per_mm[A_AXIS])),
int32_t(LROUND(b * settings.axis_steps_per_mm[B_AXIS])),
int32_t(LROUND(c * settings.axis_steps_per_mm[C_AXIS])),
@ -2593,14 +2556,14 @@ bool Planner::buffer_segment(const float &a, const float &b, const float &c, con
};
#if HAS_POSITION_FLOAT
const float target_float[XYZE] = { a, b, c, e };
const xyze_pos_t target_float = { a, b, c, e };
#endif
// DRYRUN prevents E moves from taking place
if (DEBUGGING(DRYRUN)) {
position[E_AXIS] = target[E_AXIS];
position.e = target.e;
#if HAS_POSITION_FLOAT
position_float[E_AXIS] = e;
position_float.e = e;
#endif
}
@ -2608,27 +2571,27 @@ bool Planner::buffer_segment(const float &a, const float &b, const float &c, con
SERIAL_ECHOPAIR(" buffer_segment FR:", fr_mm_s);
#if IS_KINEMATIC
SERIAL_ECHOPAIR(" A:", a);
SERIAL_ECHOPAIR(" (", position[A_AXIS]);
SERIAL_ECHOPAIR("->", target[A_AXIS]);
SERIAL_ECHOPAIR(" (", position.a);
SERIAL_ECHOPAIR("->", target.a);
SERIAL_ECHOPAIR(") B:", b);
#else
SERIAL_ECHOPAIR(" X:", a);
SERIAL_ECHOPAIR(" (", position[X_AXIS]);
SERIAL_ECHOPAIR("->", target[X_AXIS]);
SERIAL_ECHOPAIR(" (", position.x);
SERIAL_ECHOPAIR("->", target.x);
SERIAL_ECHOPAIR(") Y:", b);
#endif
SERIAL_ECHOPAIR(" (", position[Y_AXIS]);
SERIAL_ECHOPAIR("->", target[Y_AXIS]);
SERIAL_ECHOPAIR(" (", position.y);
SERIAL_ECHOPAIR("->", target.y);
#if ENABLED(DELTA)
SERIAL_ECHOPAIR(") C:", c);
#else
SERIAL_ECHOPAIR(") Z:", c);
#endif
SERIAL_ECHOPAIR(" (", position[Z_AXIS]);
SERIAL_ECHOPAIR("->", target[Z_AXIS]);
SERIAL_ECHOPAIR(" (", position.z);
SERIAL_ECHOPAIR("->", target.z);
SERIAL_ECHOPAIR(") E:", e);
SERIAL_ECHOPAIR(" (", position[E_AXIS]);
SERIAL_ECHOPAIR("->", target[E_AXIS]);
SERIAL_ECHOPAIR(" (", position.e);
SERIAL_ECHOPAIR("->", target.e);
SERIAL_ECHOLNPGM(")");
//*/
@ -2665,51 +2628,49 @@ bool Planner::buffer_line(const float &rx, const float &ry, const float &rz, con
, const float &inv_duration
#endif
) {
float raw[XYZE] = { rx, ry, rz, e };
xyze_pos_t machine = { rx, ry, rz, e };
#if HAS_POSITION_MODIFIERS
apply_modifiers(raw);
apply_modifiers(machine);
#endif
#if IS_KINEMATIC
const float delta_mm_cart[] = {
rx - position_cart[X_AXIS],
ry - position_cart[Y_AXIS],
rz - position_cart[Z_AXIS]
#if ENABLED(JUNCTION_DEVIATION)
, e - position_cart[E_AXIS]
#endif
};
#if ENABLED(JUNCTION_DEVIATION)
const xyze_pos_t delta_mm_cart = {
rx - position_cart.x, ry - position_cart.y,
rz - position_cart.z, e - position_cart.e
};
#else
const xyz_pos_t delta_mm_cart = { rx - position_cart.x, ry - position_cart.y, rz - position_cart.z };
#endif
float mm = millimeters;
if (mm == 0.0)
mm = (delta_mm_cart[X_AXIS] != 0.0 || delta_mm_cart[Y_AXIS] != 0.0) ? SQRT(sq(delta_mm_cart[X_AXIS]) + sq(delta_mm_cart[Y_AXIS]) + sq(delta_mm_cart[Z_AXIS])) : ABS(delta_mm_cart[Z_AXIS]);
mm = (delta_mm_cart.x != 0.0 || delta_mm_cart.y != 0.0) ? delta_mm_cart.magnitude() : ABS(delta_mm_cart.z);
inverse_kinematics(raw);
inverse_kinematics(machine);
#if ENABLED(SCARA_FEEDRATE_SCALING)
// For SCARA scale the feed rate from mm/s to degrees/s
// i.e., Complete the angular vector in the given time.
const float duration_recip = inv_duration ? inv_duration : fr_mm_s / mm;
const feedRate_t feedrate = HYPOT(delta[A_AXIS] - position_float[A_AXIS], delta[B_AXIS] - position_float[B_AXIS]) * duration_recip;
const feedRate_t feedrate = HYPOT(delta.a - position_float.a, delta.b - position_float.b) * duration_recip;
#else
const feedRate_t feedrate = fr_mm_s;
#endif
if (buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS]
if (buffer_segment(delta.a, delta.b, delta.c, machine.e
#if ENABLED(JUNCTION_DEVIATION)
, delta_mm_cart
#endif
, feedrate, extruder, mm
)) {
position_cart[X_AXIS] = rx;
position_cart[Y_AXIS] = ry;
position_cart[Z_AXIS] = rz;
position_cart[E_AXIS] = e;
position_cart.set(rx, ry, rz, e);
return true;
}
else
return false;
#else
return buffer_segment(raw, fr_mm_s, extruder, millimeters);
return buffer_segment(machine, fr_mm_s, extruder, millimeters);
#endif
} // buffer_line()
@ -2724,30 +2685,27 @@ void Planner::set_machine_position_mm(const float &a, const float &b, const floa
#if ENABLED(DISTINCT_E_FACTORS)
last_extruder = active_extruder;
#endif
position[A_AXIS] = LROUND(a * settings.axis_steps_per_mm[A_AXIS]);
position[B_AXIS] = LROUND(b * settings.axis_steps_per_mm[B_AXIS]);
position[C_AXIS] = LROUND(c * settings.axis_steps_per_mm[C_AXIS]);
position[E_AXIS] = LROUND(e * settings.axis_steps_per_mm[E_AXIS_N(active_extruder)]);
#if HAS_POSITION_FLOAT
position_float[A_AXIS] = a;
position_float[B_AXIS] = b;
position_float[C_AXIS] = c;
position_float[E_AXIS] = e;
position_float.set(a, b, c, e);
#endif
position.set(LROUND(a * settings.axis_steps_per_mm[A_AXIS]),
LROUND(b * settings.axis_steps_per_mm[B_AXIS]),
LROUND(c * settings.axis_steps_per_mm[C_AXIS]),
LROUND(e * settings.axis_steps_per_mm[E_AXIS_N(active_extruder)]));
if (has_blocks_queued()) {
//previous_nominal_speed_sqr = 0.0; // Reset planner junction speeds. Assume start from rest.
//ZERO(previous_speed);
//previous_speed.reset();
buffer_sync_block();
}
else
stepper.set_position(position[A_AXIS], position[B_AXIS], position[C_AXIS], position[E_AXIS]);
stepper.set_position(position);
}
void Planner::set_position_mm(const float &rx, const float &ry, const float &rz, const float &e) {
float raw[XYZE] = { rx, ry, rz, e };
xyze_pos_t machine = { rx, ry, rz, e };
#if HAS_POSITION_MODIFIERS
{
apply_modifiers(raw
apply_modifiers(machine
#if HAS_LEVELING
, true
#endif
@ -2755,15 +2713,11 @@ void Planner::set_position_mm(const float &rx, const float &ry, const float &rz,
}
#endif
#if IS_KINEMATIC
position_cart[X_AXIS] = rx;
position_cart[Y_AXIS] = ry;
position_cart[Z_AXIS] = rz;
position_cart[E_AXIS] = e;
inverse_kinematics(raw);
set_machine_position_mm(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS]);
position_cart.set(rx, ry, rz, e);
inverse_kinematics(machine);
set_machine_position_mm(delta.a, delta.b, delta.c, machine.e);
#else
set_machine_position_mm(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], raw[E_AXIS]);
set_machine_position_mm(machine);
#endif
}
@ -2780,17 +2734,17 @@ void Planner::set_e_position_mm(const float &e) {
#else
const float e_new = e;
#endif
position[E_AXIS] = LROUND(settings.axis_steps_per_mm[axis_index] * e_new);
position.e = LROUND(settings.axis_steps_per_mm[axis_index] * e_new);
#if HAS_POSITION_FLOAT
position_float[E_AXIS] = e_new;
position_float.e = e_new;
#endif
#if IS_KINEMATIC
position_cart[E_AXIS] = e;
position_cart.e = e;
#endif
if (has_blocks_queued())
buffer_sync_block();
else
stepper.set_position(E_AXIS, position[E_AXIS]);
stepper.set_axis_position(E_AXIS, position.e);
}
// Recalculate the steps/s^2 acceleration rates, based on the mm/s^2

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

@ -40,7 +40,7 @@
#endif
#if ABL_PLANAR
#include "../libs/vector_3.h"
#include "../libs/vector_3.h" // for matrix_3x3
#endif
#if ENABLED(FWRETRACT)
@ -51,6 +51,11 @@
#include "../feature/mixing.h"
#endif
// Feedrate for manual moves
#ifdef MANUAL_FEEDRATE
constexpr xyze_feedrate_t manual_feedrate_mm_m = MANUAL_FEEDRATE;
#endif
enum BlockFlagBit : char {
// Recalculate trapezoids on entry junction. For optimization.
BLOCK_BIT_RECALCULATE,
@ -95,15 +100,8 @@ typedef struct block_t {
acceleration; // acceleration mm/sec^2
union {
// Data used by all move blocks
struct {
// Fields used by the Bresenham algorithm for tracing the line
uint32_t steps[NUM_AXIS]; // Step count along each axis
};
// Data used by all sync blocks
struct {
int32_t position[NUM_AXIS]; // New position to force when this sync block is executed
};
abce_ulong_t steps; // Step count along each axis
abce_long_t position; // New position to force when this sync block is executed
};
uint32_t step_event_count; // The number of step events required to complete this block
@ -259,19 +257,18 @@ class Planner {
#endif
#if HAS_CLASSIC_JERK
static float max_jerk[
#if BOTH(JUNCTION_DEVIATION, LIN_ADVANCE)
XYZ // (mm/s^2) M205 XYZ - The largest speed change requiring no acceleration.
#else
XYZE // (mm/s^2) M205 XYZE - The largest speed change requiring no acceleration.
#endif
];
#if BOTH(JUNCTION_DEVIATION, LIN_ADVANCE)
static xyz_pos_t max_jerk; // (mm/s^2) M205 XYZ - The largest speed change requiring no acceleration.
#else
static xyze_pos_t max_jerk; // (mm/s^2) M205 XYZE - The largest speed change requiring no acceleration.
#endif
#endif
#if HAS_LEVELING
static bool leveling_active; // Flag that bed leveling is enabled
#if ABL_PLANAR
static matrix_3x3 bed_level_matrix; // Transform to compensate for bed level
static constexpr xy_pos_t level_fulcrum = { X_TILT_FULCRUM, Y_TILT_FULCRUM };
#endif
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
static float z_fade_height, inverse_z_fade_height;
@ -285,11 +282,11 @@ class Planner {
#endif
#if HAS_POSITION_FLOAT
static float position_float[XYZE];
static xyze_pos_t position_float;
#endif
#if IS_KINEMATIC
static float position_cart[XYZE];
static xyze_pos_t position_cart;
#endif
static skew_factor_t skew_factor;
@ -304,12 +301,12 @@ class Planner {
* The current position of the tool in absolute steps
* Recalculated if any axis_steps_per_mm are changed by gcode
*/
static int32_t position[NUM_AXIS];
static xyze_long_t position;
/**
* Speed of previous path line segment
*/
static float previous_speed[NUM_AXIS];
static xyze_float_t previous_speed;
/**
* Nominal speed of previous path line segment (mm/s)^2
@ -338,7 +335,7 @@ class Planner {
// Old direction bits. Used for speed calculations
static unsigned char old_direction_bits;
// Segment times (in µs). Used for speed calculations
static uint32_t axis_segment_time_us[2][3];
static xy_ulong_t axis_segment_time_us[3];
#endif
#if HAS_SPI_LCD
@ -454,8 +451,7 @@ class Planner {
}
}
}
FORCE_INLINE static void skew(float (&raw)[XYZ]) { skew(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]); }
FORCE_INLINE static void skew(float (&raw)[XYZE]) { skew(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]); }
FORCE_INLINE static void skew(xyz_pos_t &raw) { skew(raw.x, raw.y, raw.z); }
FORCE_INLINE static void unskew(float &cx, float &cy, const float &cz) {
if (WITHIN(cx, X_MIN_POS, X_MAX_POS) && WITHIN(cy, Y_MIN_POS, Y_MAX_POS)) {
@ -466,8 +462,7 @@ class Planner {
}
}
}
FORCE_INLINE static void unskew(float (&raw)[XYZ]) { unskew(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]); }
FORCE_INLINE static void unskew(float (&raw)[XYZE]) { unskew(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]); }
FORCE_INLINE static void unskew(xyz_pos_t &raw) { unskew(raw.x, raw.y, raw.z); }
#endif // SKEW_CORRECTION
@ -476,22 +471,24 @@ class Planner {
* Apply leveling to transform a cartesian position
* as it will be given to the planner and steppers.
*/
static void apply_leveling(float &rx, float &ry, float &rz);
FORCE_INLINE static void apply_leveling(float (&raw)[XYZ]) { apply_leveling(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]); }
FORCE_INLINE static void apply_leveling(float (&raw)[XYZE]) { apply_leveling(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]); }
static void unapply_leveling(float raw[XYZ]);
static void apply_leveling(xyz_pos_t &raw);
static void unapply_leveling(xyz_pos_t &raw);
FORCE_INLINE static void force_unapply_leveling(xyz_pos_t &raw) {
leveling_active = true;
unapply_leveling(raw);
leveling_active = false;
}
#endif
#if ENABLED(FWRETRACT)
static void apply_retract(float &rz, float &e);
FORCE_INLINE static void apply_retract(float (&raw)[XYZE]) { apply_retract(raw[Z_AXIS], raw[E_AXIS]); }
FORCE_INLINE static void apply_retract(xyze_pos_t &raw) { apply_retract(raw.z, raw.e); }
static void unapply_retract(float &rz, float &e);
FORCE_INLINE static void unapply_retract(float (&raw)[XYZE]) { unapply_retract(raw[Z_AXIS], raw[E_AXIS]); }
FORCE_INLINE static void unapply_retract(xyze_pos_t &raw) { unapply_retract(raw.z, raw.e); }
#endif
#if HAS_POSITION_MODIFIERS
FORCE_INLINE static void apply_modifiers(float (&pos)[XYZE]
FORCE_INLINE static void apply_modifiers(xyze_pos_t &pos
#if HAS_LEVELING
, bool leveling =
#if PLANNER_LEVELING
@ -512,7 +509,7 @@ class Planner {
#endif
}
FORCE_INLINE static void unapply_modifiers(float (&pos)[XYZE]
FORCE_INLINE static void unapply_modifiers(xyze_pos_t &pos
#if HAS_LEVELING
, bool leveling =
#if PLANNER_LEVELING
@ -578,12 +575,12 @@ class Planner {
*
* Returns true if movement was buffered, false otherwise
*/
static bool _buffer_steps(const int32_t (&target)[XYZE]
static bool _buffer_steps(const xyze_long_t &target
#if HAS_POSITION_FLOAT
, const float (&target_float)[ABCE]
, const xyze_pos_t &target_float
#endif
#if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION)
, const float (&delta_mm_cart)[XYZE]
, const xyze_float_t &delta_mm_cart
#endif
, feedRate_t fr_mm_s, const uint8_t extruder, const float &millimeters=0.0
);
@ -601,12 +598,12 @@ class Planner {
* Returns true is movement is acceptable, false otherwise
*/
static bool _populate_block(block_t * const block, bool split_move,
const int32_t (&target)[XYZE]
const xyze_long_t &target
#if HAS_POSITION_FLOAT
, const float (&target_float)[XYZE]
, const xyze_pos_t &target_float
#endif
#if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION)
, const float (&delta_mm_cart)[XYZE]
, const xyze_float_t &delta_mm_cart
#endif
, feedRate_t fr_mm_s, const uint8_t extruder, const float &millimeters=0.0
);
@ -638,18 +635,18 @@ class Planner {
*/
static bool buffer_segment(const float &a, const float &b, const float &c, const float &e
#if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION)
, const float (&delta_mm_cart)[XYZE]
, const xyze_float_t &delta_mm_cart
#endif
, const feedRate_t &fr_mm_s, const uint8_t extruder, const float &millimeters=0.0
);
FORCE_INLINE static bool buffer_segment(const float (&abce)[ABCE]
FORCE_INLINE static bool buffer_segment(abce_pos_t &abce
#if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION)
, const float (&delta_mm_cart)[XYZE]
, const xyze_float_t &delta_mm_cart
#endif
, const feedRate_t &fr_mm_s, const uint8_t extruder, const float &millimeters=0.0
) {
return buffer_segment(abce[A_AXIS], abce[B_AXIS], abce[C_AXIS], abce[E_AXIS]
return buffer_segment(abce.a, abce.b, abce.c, abce.e
#if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION)
, delta_mm_cart
#endif
@ -675,12 +672,12 @@ class Planner {
#endif
);
FORCE_INLINE static bool buffer_line(const float (&cart)[XYZE], const feedRate_t &fr_mm_s, const uint8_t extruder, const float millimeters=0.0
FORCE_INLINE static bool buffer_line(const xyze_pos_t &cart, const feedRate_t &fr_mm_s, const uint8_t extruder, const float millimeters=0.0
#if ENABLED(SCARA_FEEDRATE_SCALING)
, const float &inv_duration=0.0
#endif
) {
return buffer_line(cart[X_AXIS], cart[Y_AXIS], cart[Z_AXIS], cart[E_AXIS], fr_mm_s, extruder, millimeters
return buffer_line(cart.x, cart.y, cart.z, cart.e, fr_mm_s, extruder, millimeters
#if ENABLED(SCARA_FEEDRATE_SCALING)
, inv_duration
#endif
@ -701,7 +698,7 @@ class Planner {
* Clears previous speed values.
*/
static void set_position_mm(const float &rx, const float &ry, const float &rz, const float &e);
FORCE_INLINE static void set_position_mm(const float (&cart)[XYZE]) { set_position_mm(cart[X_AXIS], cart[Y_AXIS], cart[Z_AXIS], cart[E_AXIS]); }
FORCE_INLINE static void set_position_mm(const xyze_pos_t &cart) { set_position_mm(cart.x, cart.y, cart.z, cart.e); }
static void set_e_position_mm(const float &e);
/**
@ -711,7 +708,7 @@ class Planner {
* conversions are applied.
*/
static void set_machine_position_mm(const float &a, const float &b, const float &c, const float &e);
FORCE_INLINE static void set_machine_position_mm(const float (&abce)[ABCE]) { set_machine_position_mm(abce[A_AXIS], abce[B_AXIS], abce[C_AXIS], abce[E_AXIS]); }
FORCE_INLINE static void set_machine_position_mm(const abce_pos_t &abce) { set_machine_position_mm(abce.a, abce.b, abce.c, abce.e); }
/**
* Get an axis position according to stepper position(s)
@ -942,14 +939,13 @@ class Planner {
#if ENABLED(JUNCTION_DEVIATION)
FORCE_INLINE static void normalize_junction_vector(float (&vector)[XYZE]) {
FORCE_INLINE static void normalize_junction_vector(xyze_float_t &vector) {
float magnitude_sq = 0;
LOOP_XYZE(idx) if (vector[idx]) magnitude_sq += sq(vector[idx]);
const float inv_magnitude = RSQRT(magnitude_sq);
LOOP_XYZE(idx) vector[idx] *= inv_magnitude;
vector *= RSQRT(magnitude_sq);
}
FORCE_INLINE static float limit_value_by_axis_maximum(const float &max_value, float (&unit_vec)[XYZE]) {
FORCE_INLINE static float limit_value_by_axis_maximum(const float &max_value, xyze_float_t &unit_vec) {
float limit_value = max_value;
LOOP_XYZE(idx) if (unit_vec[idx]) // Avoid divide by zero
NOMORE(limit_value, ABS(settings.max_acceleration_mm_per_s2[idx] / unit_vec[idx]));

54
Marlin/src/module/planner_bezier.cpp
View File

@ -108,21 +108,17 @@ static inline float dist1(const float &x1, const float &y1, const float &x2, con
* power available on Arduino, I think it is not wise to implement it.
*/
void cubic_b_spline(
const float position[NUM_AXIS], // current position
const float target[NUM_AXIS], // target position
const float (&offset)[4], // a pair of offsets
const xyze_pos_t &position, // current position
const xyze_pos_t &target, // target position
const xy_pos_t (&offsets)[2], // a pair of offsets
const feedRate_t &scaled_fr_mm_s, // mm/s scaled by feedrate %
const uint8_t extruder
) {
// Absolute first and second control points are recovered.
const float first0 = position[X_AXIS] + offset[0],
first1 = position[Y_AXIS] + offset[1],
second0 = target[X_AXIS] + offset[2],
second1 = target[Y_AXIS] + offset[3];
const xy_pos_t first = position + offsets[0], second = target + offsets[1];
float bez_target[4];
bez_target[X_AXIS] = position[X_AXIS];
bez_target[Y_AXIS] = position[Y_AXIS];
xyze_pos_t bez_target;
bez_target.set(position.x, position.y);
float step = MAX_STEP;
millis_t next_idle_ms = millis() + 200UL;
@ -141,15 +137,15 @@ void cubic_b_spline(
bool did_reduce = false;
float new_t = t + step;
NOMORE(new_t, 1);
float new_pos0 = eval_bezier(position[X_AXIS], first0, second0, target[X_AXIS], new_t),
new_pos1 = eval_bezier(position[Y_AXIS], first1, second1, target[Y_AXIS], new_t);
float new_pos0 = eval_bezier(position.x, first.x, second.x, target.x, new_t),
new_pos1 = eval_bezier(position.y, first.y, second.y, target.y, new_t);
for (;;) {
if (new_t - t < (MIN_STEP)) break;
const float candidate_t = 0.5f * (t + new_t),
candidate_pos0 = eval_bezier(position[X_AXIS], first0, second0, target[X_AXIS], candidate_t),
candidate_pos1 = eval_bezier(position[Y_AXIS], first1, second1, target[Y_AXIS], candidate_t),
interp_pos0 = 0.5f * (bez_target[X_AXIS] + new_pos0),
interp_pos1 = 0.5f * (bez_target[Y_AXIS] + new_pos1);
candidate_pos0 = eval_bezier(position.x, first.x, second.x, target.x, candidate_t),
candidate_pos1 = eval_bezier(position.y, first.y, second.y, target.y, candidate_t),
interp_pos0 = 0.5f * (bez_target.x + new_pos0),
interp_pos1 = 0.5f * (bez_target.y + new_pos1);
if (dist1(candidate_pos0, candidate_pos1, interp_pos0, interp_pos1) <= (SIGMA)) break;
new_t = candidate_t;
new_pos0 = candidate_pos0;
@ -162,10 +158,10 @@ void cubic_b_spline(
if (new_t - t > MAX_STEP) break;
const float candidate_t = t + 2 * (new_t - t);
if (candidate_t >= 1) break;
const float candidate_pos0 = eval_bezier(position[X_AXIS], first0, second0, target[X_AXIS], candidate_t),
candidate_pos1 = eval_bezier(position[Y_AXIS], first1, second1, target[Y_AXIS], candidate_t),
interp_pos0 = 0.5f * (bez_target[X_AXIS] + candidate_pos0),
interp_pos1 = 0.5f * (bez_target[Y_AXIS] + candidate_pos1);
const float candidate_pos0 = eval_bezier(position.x, first.x, second.x, target.x, candidate_t),
candidate_pos1 = eval_bezier(position.y, first.y, second.y, target.y, candidate_t),
interp_pos0 = 0.5f * (bez_target.x + candidate_pos0),
interp_pos1 = 0.5f * (bez_target.y + candidate_pos1);
if (dist1(new_pos0, new_pos1, interp_pos0, interp_pos1) > (SIGMA)) break;
new_t = candidate_t;
new_pos0 = candidate_pos0;
@ -187,19 +183,19 @@ void cubic_b_spline(
t = new_t;
// Compute and send new position
bez_target[X_AXIS] = new_pos0;
bez_target[Y_AXIS] = new_pos1;
// FIXME. The following two are wrong, since the parameter t is
// not linear in the distance.
bez_target[Z_AXIS] = interp(position[Z_AXIS], target[Z_AXIS], t);
bez_target[E_AXIS] = interp(position[E_AXIS], target[E_AXIS], t);
apply_motion_limits(bez_target);
xyze_pos_t new_bez = {
new_pos0, new_pos1,
interp(position.z, target.z, t), // FIXME. These two are wrong, since the parameter t is
interp(position.e, target.e, t) // not linear in the distance.
};
apply_motion_limits(new_bez);
bez_target = new_bez;
#if HAS_LEVELING && !PLANNER_LEVELING
float pos[XYZE] = { bez_target[X_AXIS], bez_target[Y_AXIS], bez_target[Z_AXIS], bez_target[E_AXIS] };
xyze_pos_t pos = bez_target;
planner.apply_leveling(pos);
#else
const float (&pos)[XYZE] = bez_target;
const xyze_pos_t &pos = bez_target;
#endif
if (!planner.buffer_line(pos, scaled_fr_mm_s, active_extruder, step))

9
Marlin/src/module/planner_bezier.h
View File

@ -28,13 +28,12 @@
*
*/
#include <stdint.h>
#include "../core/macros.h"
#include "../core/types.h"
void cubic_b_spline(
const float position[NUM_AXIS], // current position
const float target[NUM_AXIS], // target position
const float (&offset)[4], // a pair of offsets
const xyze_pos_t &position, // current position
const xyze_pos_t &target, // target position
const xy_pos_t (&offsets)[2], // a pair of offsets
const feedRate_t &scaled_fr_mm_s, // mm/s scaled by feedrate %
const uint8_t extruder
);

80
Marlin/src/module/probe.cpp
View File

@ -56,7 +56,7 @@
#include "../feature/backlash.h"
#endif
float probe_offset[XYZ]; // Initialized by settings.load()
xyz_pos_t probe_offset; // Initialized by settings.load()
#if ENABLED(BLTOUCH)
#include "../feature/bltouch.h"
@ -146,10 +146,10 @@ float probe_offset[XYZ]; // Initialized by settings.load()
// Move down to the bed to stow the probe
void run_stow_moves_script() {
const float old_pos[] = { current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] };
const xyz_pos_t oldpos = current_position;
endstops.enable_z_probe(false);
do_blocking_move_to_z(TOUCH_MI_RETRACT_Z, MMM_TO_MMS(HOMING_FEEDRATE_Z));
do_blocking_move_to(old_pos, MMM_TO_MMS(HOMING_FEEDRATE_Z));
do_blocking_move_to(oldpos, MMM_TO_MMS(HOMING_FEEDRATE_Z));
}
#elif ENABLED(Z_PROBE_ALLEN_KEY)
@ -159,35 +159,35 @@ float probe_offset[XYZ]; // Initialized by settings.load()
#ifndef Z_PROBE_ALLEN_KEY_DEPLOY_1_FEEDRATE
#define Z_PROBE_ALLEN_KEY_DEPLOY_1_FEEDRATE 0.0
#endif
constexpr float deploy_1[] = Z_PROBE_ALLEN_KEY_DEPLOY_1;
constexpr xyz_pos_t deploy_1 = Z_PROBE_ALLEN_KEY_DEPLOY_1;
do_blocking_move_to(deploy_1, MMM_TO_MMS(Z_PROBE_ALLEN_KEY_DEPLOY_1_FEEDRATE));
#endif
#ifdef Z_PROBE_ALLEN_KEY_DEPLOY_2
#ifndef Z_PROBE_ALLEN_KEY_DEPLOY_2_FEEDRATE
#define Z_PROBE_ALLEN_KEY_DEPLOY_2_FEEDRATE 0.0
#endif
constexpr float deploy_2[] = Z_PROBE_ALLEN_KEY_DEPLOY_2;
constexpr xyz_pos_t deploy_2 = Z_PROBE_ALLEN_KEY_DEPLOY_2;
do_blocking_move_to(deploy_2, MMM_TO_MMS(Z_PROBE_ALLEN_KEY_DEPLOY_2_FEEDRATE));
#endif
#ifdef Z_PROBE_ALLEN_KEY_DEPLOY_3
#ifndef Z_PROBE_ALLEN_KEY_DEPLOY_3_FEEDRATE
#define Z_PROBE_ALLEN_KEY_DEPLOY_3_FEEDRATE 0.0
#endif
constexpr float deploy_3[] = Z_PROBE_ALLEN_KEY_DEPLOY_3;
constexpr xyz_pos_t deploy_3 = Z_PROBE_ALLEN_KEY_DEPLOY_3;
do_blocking_move_to(deploy_3, MMM_TO_MMS(Z_PROBE_ALLEN_KEY_DEPLOY_3_FEEDRATE));
#endif
#ifdef Z_PROBE_ALLEN_KEY_DEPLOY_4
#ifndef Z_PROBE_ALLEN_KEY_DEPLOY_4_FEEDRATE
#define Z_PROBE_ALLEN_KEY_DEPLOY_4_FEEDRATE 0.0
#endif
constexpr float deploy_4[] = Z_PROBE_ALLEN_KEY_DEPLOY_4;
constexpr xyz_pos_t deploy_4 = Z_PROBE_ALLEN_KEY_DEPLOY_4;
do_blocking_move_to(deploy_4, MMM_TO_MMS(Z_PROBE_ALLEN_KEY_DEPLOY_4_FEEDRATE));
#endif
#ifdef Z_PROBE_ALLEN_KEY_DEPLOY_5
#ifndef Z_PROBE_ALLEN_KEY_DEPLOY_5_FEEDRATE
#define Z_PROBE_ALLEN_KEY_DEPLOY_5_FEEDRATE 0.0
#endif
constexpr float deploy_5[] = Z_PROBE_ALLEN_KEY_DEPLOY_5;
constexpr xyz_pos_t deploy_5 = Z_PROBE_ALLEN_KEY_DEPLOY_5;
do_blocking_move_to(deploy_5, MMM_TO_MMS(Z_PROBE_ALLEN_KEY_DEPLOY_5_FEEDRATE));
#endif
}
@ -197,35 +197,35 @@ float probe_offset[XYZ]; // Initialized by settings.load()
#ifndef Z_PROBE_ALLEN_KEY_STOW_1_FEEDRATE
#define Z_PROBE_ALLEN_KEY_STOW_1_FEEDRATE 0.0
#endif
constexpr float stow_1[] = Z_PROBE_ALLEN_KEY_STOW_1;
constexpr xyz_pos_t stow_1 = Z_PROBE_ALLEN_KEY_STOW_1;
do_blocking_move_to(stow_1, MMM_TO_MMS(Z_PROBE_ALLEN_KEY_STOW_1_FEEDRATE));
#endif
#ifdef Z_PROBE_ALLEN_KEY_STOW_2
#ifndef Z_PROBE_ALLEN_KEY_STOW_2_FEEDRATE
#define Z_PROBE_ALLEN_KEY_STOW_2_FEEDRATE 0.0
#endif
constexpr float stow_2[] = Z_PROBE_ALLEN_KEY_STOW_2;
constexpr xyz_pos_t stow_2 = Z_PROBE_ALLEN_KEY_STOW_2;
do_blocking_move_to(stow_2, MMM_TO_MMS(Z_PROBE_ALLEN_KEY_STOW_2_FEEDRATE));
#endif
#ifdef Z_PROBE_ALLEN_KEY_STOW_3
#ifndef Z_PROBE_ALLEN_KEY_STOW_3_FEEDRATE
#define Z_PROBE_ALLEN_KEY_STOW_3_FEEDRATE 0.0
#endif
constexpr float stow_3[] = Z_PROBE_ALLEN_KEY_STOW_3;
constexpr xyz_pos_t stow_3 = Z_PROBE_ALLEN_KEY_STOW_3;
do_blocking_move_to(stow_3, MMM_TO_MMS(Z_PROBE_ALLEN_KEY_STOW_3_FEEDRATE));
#endif
#ifdef Z_PROBE_ALLEN_KEY_STOW_4
#ifndef Z_PROBE_ALLEN_KEY_STOW_4_FEEDRATE
#define Z_PROBE_ALLEN_KEY_STOW_4_FEEDRATE 0.0
#endif
constexpr float stow_4[] = Z_PROBE_ALLEN_KEY_STOW_4;
constexpr xyz_pos_t stow_4 = Z_PROBE_ALLEN_KEY_STOW_4;
do_blocking_move_to(stow_4, MMM_TO_MMS(Z_PROBE_ALLEN_KEY_STOW_4_FEEDRATE));
#endif
#ifdef Z_PROBE_ALLEN_KEY_STOW_5
#ifndef Z_PROBE_ALLEN_KEY_STOW_5_FEEDRATE
#define Z_PROBE_ALLEN_KEY_STOW_5_FEEDRATE 0.0
#endif
constexpr float stow_5[] = Z_PROBE_ALLEN_KEY_STOW_5;
constexpr xyz_pos_t stow_5 = Z_PROBE_ALLEN_KEY_STOW_5;
do_blocking_move_to(stow_5, MMM_TO_MMS(Z_PROBE_ALLEN_KEY_STOW_5_FEEDRATE));
#endif
}
@ -263,11 +263,11 @@ inline void do_probe_raise(const float z_raise) {
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("do_probe_raise(", z_raise, ")");
float z_dest = z_raise;
if (probe_offset[Z_AXIS] < 0) z_dest -= probe_offset[Z_AXIS];
if (probe_offset.z < 0) z_dest -= probe_offset.z;
NOMORE(z_dest, Z_MAX_POS);
if (z_dest > current_position[Z_AXIS])
if (z_dest > current_position.z)
do_blocking_move_to_z(z_dest);
}
@ -384,8 +384,7 @@ bool set_probe_deployed(const bool deploy) {
}
#endif
const float oldXpos = current_position[X_AXIS],
oldYpos = current_position[Y_AXIS];
const xy_pos_t old_xy = current_position;
#if ENABLED(PROBE_TRIGGERED_WHEN_STOWED_TEST)
#if USES_Z_MIN_PROBE_ENDSTOP
@ -419,7 +418,7 @@ bool set_probe_deployed(const bool deploy) {
#endif
do_blocking_move_to(oldXpos, oldYpos, current_position[Z_AXIS]); // return to position before deploy
do_blocking_move_to(old_xy);
endstops.enable_z_probe(deploy);
return false;
}
@ -427,9 +426,9 @@ bool set_probe_deployed(const bool deploy) {
#ifdef Z_AFTER_PROBING
// After probing move to a preferred Z position
void move_z_after_probing() {
if (current_position[Z_AXIS] != Z_AFTER_PROBING) {
if (current_position.z != Z_AFTER_PROBING) {
do_blocking_move_to_z(Z_AFTER_PROBING);
current_position[Z_AXIS] = Z_AFTER_PROBING;
current_position.z = Z_AFTER_PROBING;
}
}
#endif
@ -532,7 +531,7 @@ static bool do_probe_move(const float z, const feedRate_t fr_mm_s) {
* @brief Probe at the current XY (possibly more than once) to find the bed Z.
*
* @details Used by probe_at_point to get the bed Z height at the current XY.
* Leaves current_position[Z_AXIS] at the height where the probe triggered.
* Leaves current_position.z at the height where the probe triggered.
*
* @return The Z position of the bed at the current XY or NAN on error.
*/
@ -542,7 +541,7 @@ static float run_z_probe() {
// Stop the probe before it goes too low to prevent damage.
// If Z isn't known then probe to -10mm.
const float z_probe_low_point = TEST(axis_known_position, Z_AXIS) ? -probe_offset[Z_AXIS] + Z_PROBE_LOW_POINT : -10.0;
const float z_probe_low_point = TEST(axis_known_position, Z_AXIS) ? -probe_offset.z + Z_PROBE_LOW_POINT : -10.0;
// Double-probing does a fast probe followed by a slow probe
#if TOTAL_PROBING == 2
@ -556,22 +555,22 @@ static float run_z_probe() {
return NAN;
}
const float first_probe_z = current_position[Z_AXIS];
const float first_probe_z = current_position.z;
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("1st Probe Z:", first_probe_z);
// Raise to give the probe clearance
do_blocking_move_to_z(current_position[Z_AXIS] + Z_CLEARANCE_MULTI_PROBE, MMM_TO_MMS(Z_PROBE_SPEED_FAST));
do_blocking_move_to_z(current_position.z + Z_CLEARANCE_MULTI_PROBE, MMM_TO_MMS(Z_PROBE_SPEED_FAST));
#elif Z_PROBE_SPEED_FAST != Z_PROBE_SPEED_SLOW
// If the nozzle is well over the travel height then
// move down quickly before doing the slow probe
const float z = Z_CLEARANCE_DEPLOY_PROBE + 5.0 + (probe_offset[Z_AXIS] < 0 ? -probe_offset[Z_AXIS] : 0);
if (current_position[Z_AXIS] > z) {
const float z = Z_CLEARANCE_DEPLOY_PROBE + 5.0 + (probe_offset.z < 0 ? -probe_offset.z : 0);
if (current_position.z > z) {
// Probe down fast. If the probe never triggered, raise for probe clearance
if (!do_probe_move(z, MMM_TO_MMS(Z_PROBE_SPEED_FAST)))
do_blocking_move_to_z(current_position[Z_AXIS] + Z_CLEARANCE_BETWEEN_PROBES, MMM_TO_MMS(Z_PROBE_SPEED_FAST));
do_blocking_move_to_z(current_position.z + Z_CLEARANCE_BETWEEN_PROBES, MMM_TO_MMS(Z_PROBE_SPEED_FAST));
}
#endif
@ -603,7 +602,7 @@ static float run_z_probe() {
backlash.measure_with_probe();
#endif
const float z = current_position[Z_AXIS];
const float z = current_position.z;
#if EXTRA_PROBING
// Insert Z measurement into probes[]. Keep it sorted ascending.
@ -654,7 +653,7 @@ static float run_z_probe() {
#elif TOTAL_PROBING == 2
const float z2 = current_position[Z_AXIS];
const float z2 = current_position.z;
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("2nd Probe Z:", z2, " Discrepancy:", first_probe_z - z2);
@ -664,7 +663,7 @@ static float run_z_probe() {
#else
// Return the single probe result
const float measured_z = current_position[Z_AXIS];
const float measured_z = current_position.z;
#endif
@ -694,20 +693,19 @@ float probe_at_point(const float &rx, const float &ry, const ProbePtRaise raise_
}
// TODO: Adapt for SCARA, where the offset rotates
float nx = rx, ny = ry;
xyz_pos_t npos = { rx, ry };
if (probe_relative) {
if (!position_is_reachable_by_probe(rx, ry)) return NAN; // The given position is in terms of the probe
nx -= probe_offset[X_AXIS]; // Get the nozzle position
ny -= probe_offset[Y_AXIS];
if (!position_is_reachable_by_probe(npos)) return NAN; // The given position is in terms of the probe
npos -= probe_offset; // Get the nozzle position
}
else if (!position_is_reachable(nx, ny)) return NAN; // The given position is in terms of the nozzle
else if (!position_is_reachable(npos)) return NAN; // The given position is in terms of the nozzle
const float nz =
npos.z =
#if ENABLED(DELTA)
// Move below clip height or xy move will be aborted by do_blocking_move_to
_MIN(current_position[Z_AXIS], delta_clip_start_height)
_MIN(current_position.z, delta_clip_start_height)
#else
current_position[Z_AXIS]
current_position.z
#endif
;
@ -715,15 +713,15 @@ float probe_at_point(const float &rx, const float &ry, const ProbePtRaise raise_
feedrate_mm_s = XY_PROBE_FEEDRATE_MM_S;
// Move the probe to the starting XYZ
do_blocking_move_to(nx, ny, nz);
do_blocking_move_to(npos);
float measured_z = NAN;
if (!DEPLOY_PROBE()) {
measured_z = run_z_probe() + probe_offset[Z_AXIS];
measured_z = run_z_probe() + probe_offset.z;
const bool big_raise = raise_after == PROBE_PT_BIG_RAISE;
if (big_raise || raise_after == PROBE_PT_RAISE)
do_blocking_move_to_z(current_position[Z_AXIS] + (big_raise ? 25 : Z_CLEARANCE_BETWEEN_PROBES), MMM_TO_MMS(Z_PROBE_SPEED_FAST));
do_blocking_move_to_z(current_position.z + (big_raise ? 25 : Z_CLEARANCE_BETWEEN_PROBES), MMM_TO_MMS(Z_PROBE_SPEED_FAST));
else if (raise_after == PROBE_PT_STOW)
if (STOW_PROBE()) measured_z = NAN;
}

18
Marlin/src/module/probe.h
View File

@ -29,9 +29,9 @@
#if HAS_BED_PROBE
constexpr float nozzle_to_probe_offset[XYZ] = NOZZLE_TO_PROBE_OFFSET;
constexpr xyz_pos_t nozzle_to_probe_offset = NOZZLE_TO_PROBE_OFFSET;
extern float probe_offset[XYZ];
extern xyz_pos_t probe_offset;
bool set_probe_deployed(const bool deploy);
#ifdef Z_AFTER_PROBING
@ -44,6 +44,9 @@
PROBE_PT_BIG_RAISE // Raise to big clearance after run_z_probe
};
float probe_at_point(const float &rx, const float &ry, const ProbePtRaise raise_after=PROBE_PT_NONE, const uint8_t verbose_level=0, const bool probe_relative=true);
inline float probe_at_point(const xy_pos_t &pos, const ProbePtRaise raise_after=PROBE_PT_NONE, const uint8_t verbose_level=0, const bool probe_relative=true) {
return probe_at_point(pos.x, pos.y, raise_after, verbose_level, probe_relative);
}
#define DEPLOY_PROBE() set_probe_deployed(true)
#define STOW_PROBE() set_probe_deployed(false)
#if HAS_HEATED_BED && ENABLED(WAIT_FOR_BED_HEATER)
@ -52,7 +55,8 @@
#else
constexpr float probe_offset[XYZ] = { 0 };
constexpr xyz_pos_t probe_offset{0};
#define DEPLOY_PROBE()
#define STOW_PROBE()
@ -64,7 +68,7 @@
#if IS_KINEMATIC
PROBE_X_MIN, MESH_MIN_X
#else
(X_MIN_BED) + (MIN_PROBE_EDGE_LEFT), (X_MIN_POS) + probe_offset[X_AXIS]
(X_MIN_BED) + (MIN_PROBE_EDGE_LEFT), (X_MIN_POS) + probe_offset.x
#endif
);
}
@ -73,7 +77,7 @@
#if IS_KINEMATIC
PROBE_X_MAX, MESH_MAX_X
#else
(X_MAX_BED) - (MIN_PROBE_EDGE_RIGHT), (X_MAX_POS) + probe_offset[X_AXIS]
(X_MAX_BED) - (MIN_PROBE_EDGE_RIGHT), (X_MAX_POS) + probe_offset.x
#endif
);
}
@ -82,7 +86,7 @@
#if IS_KINEMATIC
PROBE_Y_MIN, MESH_MIN_Y
#else
(Y_MIN_BED) + (MIN_PROBE_EDGE_FRONT), (Y_MIN_POS) + probe_offset[Y_AXIS]
(Y_MIN_BED) + (MIN_PROBE_EDGE_FRONT), (Y_MIN_POS) + probe_offset.y
#endif
);
}
@ -91,7 +95,7 @@
#if IS_KINEMATIC
PROBE_Y_MAX, MESH_MAX_Y
#else
(Y_MAX_BED) - (MIN_PROBE_EDGE_BACK), (Y_MAX_POS) + probe_offset[Y_AXIS]
(Y_MAX_BED) - (MIN_PROBE_EDGE_BACK), (Y_MAX_POS) + probe_offset.y
#endif
);
}

43
Marlin/src/module/scara.cpp
View File

@ -36,25 +36,25 @@ float delta_segments_per_second = SCARA_SEGMENTS_PER_SECOND;
void scara_set_axis_is_at_home(const AxisEnum axis) {
if (axis == Z_AXIS)
current_position[Z_AXIS] = Z_HOME_POS;
current_position.z = Z_HOME_POS;
else {
/**
* SCARA homes XY at the same time
*/
float homeposition[XYZ];
xyz_pos_t homeposition;
LOOP_XYZ(i) homeposition[i] = base_home_pos((AxisEnum)i);
// SERIAL_ECHOLNPAIR("homeposition X:", homeposition[X_AXIS], " Y:", homeposition[Y_AXIS]);
// SERIAL_ECHOLNPAIR("homeposition X:", homeposition.x, " Y:", homeposition.y);
/**
* Get Home position SCARA arm angles using inverse kinematics,
* and calculate homing offset using forward kinematics
*/
inverse_kinematics(homeposition);
forward_kinematics_SCARA(delta[A_AXIS], delta[B_AXIS]);
forward_kinematics_SCARA(delta.a, delta.b);
// SERIAL_ECHOLNPAIR("Cartesian X:", cartes[X_AXIS], " Y:", cartes[Y_AXIS]);
// SERIAL_ECHOLNPAIR("Cartesian X:", cartes.x, " Y:", cartes.y);
current_position[axis] = cartes[axis];
@ -62,8 +62,10 @@ void scara_set_axis_is_at_home(const AxisEnum axis) {
}
}
static constexpr xy_pos_t scara_offset = { SCARA_OFFSET_X, SCARA_OFFSET_Y };
/**
* Morgan SCARA Forward Kinematics. Results in cartes[].
* Morgan SCARA Forward Kinematics. Results in 'cartes'.
* Maths and first version by QHARLEY.
* Integrated into Marlin and slightly restructured by Joachim Cerny.
*/
@ -74,8 +76,8 @@ void forward_kinematics_SCARA(const float &a, const float &b) {
b_sin = sin(RADIANS(b)) * L2,
b_cos = cos(RADIANS(b)) * L2;
cartes[X_AXIS] = a_cos + b_cos + SCARA_OFFSET_X; //theta
cartes[Y_AXIS] = a_sin + b_sin + SCARA_OFFSET_Y; //theta+phi
cartes.set(a_cos + b_cos + scara_offset.x, // theta
a_sin + b_sin + scara_offset.y); // theta+phi
/*
SERIAL_ECHOLNPAIR(
@ -86,31 +88,32 @@ void forward_kinematics_SCARA(const float &a, const float &b) {
" b_sin=", b_sin,
" b_cos=", b_cos
);
SERIAL_ECHOLNPAIR(" cartes (X,Y) = "(cartes[X_AXIS], ", ", cartes[Y_AXIS], ")");
SERIAL_ECHOLNPAIR(" cartes (X,Y) = "(cartes.x, ", ", cartes.y, ")");
//*/
}
/**
* Morgan SCARA Inverse Kinematics. Results in delta[].
* Morgan SCARA Inverse Kinematics. Results in 'delta'.
*
* See http://forums.reprap.org/read.php?185,283327
*
* Maths and first version by QHARLEY.
* Integrated into Marlin and slightly restructured by Joachim Cerny.
*/
void inverse_kinematics(const float (&raw)[XYZ]) {
void inverse_kinematics(const xyz_pos_t &raw) {
static float C2, S2, SK1, SK2, THETA, PSI;
float C2, S2, SK1, SK2, THETA, PSI;
float sx = raw[X_AXIS] - SCARA_OFFSET_X, // Translate SCARA to standard X Y
sy = raw[Y_AXIS] - SCARA_OFFSET_Y; // With scaling factor.
// Translate SCARA to standard XY with scaling factor
const xy_pos_t spos = raw - scara_offset;
const float H2 = HYPOT2(spos.x, spos.y);
if (L1 == L2)
C2 = HYPOT2(sx, sy) / L1_2_2 - 1;
C2 = H2 / L1_2_2 - 1;
else
C2 = (HYPOT2(sx, sy) - (L1_2 + L2_2)) / (2.0 * L1 * L2);
C2 = (H2 - (L1_2 + L2_2)) / (2.0 * L1 * L2);
S2 = SQRT(1 - sq(C2));
S2 = SQRT(1.0f - sq(C2));
// Unrotated Arm1 plus rotated Arm2 gives the distance from Center to End
SK1 = L1 + L2 * C2;
@ -119,14 +122,12 @@ void inverse_kinematics(const float (&raw)[XYZ]) {
SK2 = L2 * S2;
// Angle of Arm1 is the difference between Center-to-End angle and the Center-to-Elbow
THETA = ATAN2(SK1, SK2) - ATAN2(sx, sy);
THETA = ATAN2(SK1, SK2) - ATAN2(spos.x, spos.y);
// Angle of Arm2
PSI = ATAN2(S2, C2);
delta[A_AXIS] = DEGREES(THETA); // theta is support arm angle
delta[B_AXIS] = DEGREES(THETA + PSI); // equal to sub arm angle (inverted motor)
delta[C_AXIS] = raw[Z_AXIS];
delta.set(DEGREES(THETA), DEGREES(THETA + PSI), raw.z);
/*
DEBUG_POS("SCARA IK", raw);

6
Marlin/src/module/scara.h
View File

@ -36,11 +36,7 @@ float constexpr L1 = SCARA_LINKAGE_1, L2 = SCARA_LINKAGE_2,
void scara_set_axis_is_at_home(const AxisEnum axis);
void inverse_kinematics(const float (&raw)[XYZ]);
FORCE_INLINE void inverse_kinematics(const float (&raw)[XYZE]) {
const float raw_xyz[XYZ] = { raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS] };
inverse_kinematics(raw_xyz);
}
void inverse_kinematics(const xyz_pos_t &raw);
void forward_kinematics_SCARA(const float &a, const float &b);
void scara_report_positions();

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

@ -169,10 +169,10 @@ uint8_t Stepper::steps_per_isr;
#endif
uint8_t Stepper::oversampling_factor;
int32_t Stepper::delta_error[XYZE] = { 0 };
xyze_long_t Stepper::delta_error{0};
uint32_t Stepper::advance_dividend[XYZE] = { 0 },
Stepper::advance_divisor = 0,
xyze_ulong_t Stepper::advance_dividend{0};
uint32_t Stepper::advance_divisor = 0,
Stepper::step_events_completed = 0, // The number of step events executed in the current block
Stepper::accelerate_until, // The count at which to stop accelerating
Stepper::decelerate_after, // The count at which to start decelerating
@ -218,10 +218,9 @@ int32_t Stepper::ticks_nominal = -1;
uint32_t Stepper::acc_step_rate; // needed for deceleration start point
#endif
volatile int32_t Stepper::endstops_trigsteps[XYZ];
volatile int32_t Stepper::count_position[NUM_AXIS] = { 0 };
int8_t Stepper::count_direction[NUM_AXIS] = { 0, 0, 0, 0 };
xyz_long_t Stepper::endstops_trigsteps;
xyze_long_t Stepper::count_position{0};
xyze_int8_t Stepper::count_direction{0};
#define DUAL_ENDSTOP_APPLY_STEP(A,V) \
if (separate_multi_axis) { \
@ -390,20 +389,20 @@ void Stepper::set_directions() {
// what e-steppers will step. Likely all. Set all.
if (motor_direction(E_AXIS)) {
MIXER_STEPPER_LOOP(j) REV_E_DIR(j);
count_direction[E_AXIS] = -1;
count_direction.e = -1;
}
else {
MIXER_STEPPER_LOOP(j) NORM_E_DIR(j);
count_direction[E_AXIS] = 1;
count_direction.e = 1;
}
#else
if (motor_direction(E_AXIS)) {
REV_E_DIR(stepper_extruder);
count_direction[E_AXIS] = -1;
count_direction.e = -1;
}
else {
NORM_E_DIR(stepper_extruder);
count_direction[E_AXIS] = 1;
count_direction.e = 1;
}
#endif
#endif // !LIN_ADVANCE
@ -1459,15 +1458,15 @@ void Stepper::stepper_pulse_phase_isr() {
// Pulse Extruders
// Tick the E axis, correct error term and update position
#if EITHER(LIN_ADVANCE, MIXING_EXTRUDER)
delta_error[E_AXIS] += advance_dividend[E_AXIS];
if (delta_error[E_AXIS] >= 0) {
count_position[E_AXIS] += count_direction[E_AXIS];
delta_error.e += advance_dividend.e;
if (delta_error.e >= 0) {
count_position.e += count_direction.e;
#if ENABLED(LIN_ADVANCE)
delta_error[E_AXIS] -= advance_divisor;
delta_error.e -= advance_divisor;
// Don't step E here - But remember the number of steps to perform
motor_direction(E_AXIS) ? --LA_steps : ++LA_steps;
#else // !LIN_ADVANCE && MIXING_EXTRUDER
// Don't adjust delta_error[E_AXIS] here!
// Don't adjust delta_error.e here!
// Being positive is the criteria for ending the pulse.
E_STEP_WRITE(mixer.get_next_stepper(), !INVERT_E_STEP_PIN);
#endif
@ -1504,8 +1503,8 @@ void Stepper::stepper_pulse_phase_isr() {
#if DISABLED(LIN_ADVANCE)
#if ENABLED(MIXING_EXTRUDER)
if (delta_error[E_AXIS] >= 0) {
delta_error[E_AXIS] -= advance_divisor;
if (delta_error.e >= 0) {
delta_error.e -= advance_divisor;
E_STEP_WRITE(mixer.get_stepper(), INVERT_E_STEP_PIN);
}
#else // !MIXING_EXTRUDER
@ -1660,10 +1659,7 @@ uint32_t Stepper::stepper_block_phase_isr() {
// Sync block? Sync the stepper counts and return
while (TEST(current_block->flag, BLOCK_BIT_SYNC_POSITION)) {
_set_position(
current_block->position[A_AXIS], current_block->position[B_AXIS],
current_block->position[C_AXIS], current_block->position[E_AXIS]
);
_set_position(current_block->position);
planner.discard_current_block();
// Try to get a new block
@ -1698,7 +1694,7 @@ uint32_t Stepper::stepper_block_phase_isr() {
#endif
#define X_MOVE_TEST ( S_(1) != S_(2) || (S_(1) > 0 && D_(1) X_CMP D_(2)) )
#else
#define X_MOVE_TEST !!current_block->steps[A_AXIS]
#define X_MOVE_TEST !!current_block->steps.a
#endif
#if CORE_IS_XY || CORE_IS_YZ
@ -1716,7 +1712,7 @@ uint32_t Stepper::stepper_block_phase_isr() {
#endif
#define Y_MOVE_TEST ( S_(1) != S_(2) || (S_(1) > 0 && D_(1) Y_CMP D_(2)) )
#else
#define Y_MOVE_TEST !!current_block->steps[B_AXIS]
#define Y_MOVE_TEST !!current_block->steps.b
#endif
#if CORE_IS_XZ || CORE_IS_YZ
@ -1734,17 +1730,17 @@ uint32_t Stepper::stepper_block_phase_isr() {
#endif
#define Z_MOVE_TEST ( S_(1) != S_(2) || (S_(1) > 0 && D_(1) Z_CMP D_(2)) )
#else
#define Z_MOVE_TEST !!current_block->steps[C_AXIS]
#define Z_MOVE_TEST !!current_block->steps.c
#endif
uint8_t axis_bits = 0;
if (X_MOVE_TEST) SBI(axis_bits, A_AXIS);
if (Y_MOVE_TEST) SBI(axis_bits, B_AXIS);
if (Z_MOVE_TEST) SBI(axis_bits, C_AXIS);
//if (!!current_block->steps[E_AXIS]) SBI(axis_bits, E_AXIS);
//if (!!current_block->steps[A_AXIS]) SBI(axis_bits, X_HEAD);
//if (!!current_block->steps[B_AXIS]) SBI(axis_bits, Y_HEAD);
//if (!!current_block->steps[C_AXIS]) SBI(axis_bits, Z_HEAD);
//if (!!current_block->steps.e) SBI(axis_bits, E_AXIS);
//if (!!current_block->steps.a) SBI(axis_bits, X_HEAD);
//if (!!current_block->steps.b) SBI(axis_bits, Y_HEAD);
//if (!!current_block->steps.c) SBI(axis_bits, Z_HEAD);
axis_did_move = axis_bits;
// No acceleration / deceleration time elapsed so far
@ -1767,15 +1763,10 @@ uint32_t Stepper::stepper_block_phase_isr() {
step_event_count = current_block->step_event_count << oversampling;
// Initialize Bresenham delta errors to 1/2
delta_error[X_AXIS] = delta_error[Y_AXIS] = delta_error[Z_AXIS] = delta_error[E_AXIS] = -int32_t(step_event_count);
delta_error = -int32_t(step_event_count);
// Calculate Bresenham dividends
advance_dividend[X_AXIS] = current_block->steps[X_AXIS] << 1;
advance_dividend[Y_AXIS] = current_block->steps[Y_AXIS] << 1;
advance_dividend[Z_AXIS] = current_block->steps[Z_AXIS] << 1;
advance_dividend[E_AXIS] = current_block->steps[E_AXIS] << 1;
// Calculate Bresenham divisor
// Calculate Bresenham dividends and divisors
advance_dividend = current_block->steps << 1;
advance_divisor = step_event_count << 1;
// No step events completed so far
@ -1840,7 +1831,7 @@ uint32_t Stepper::stepper_block_phase_isr() {
// If delayed Z enable, enable it now. This option will severely interfere with
// timing between pulses when chaining motion between blocks, and it could lead
// to lost steps in both X and Y axis, so avoid using it unless strictly necessary!!
if (current_block->steps[Z_AXIS]) enable_Z();
if (current_block->steps.z) enable_Z();
#endif
// Mark the time_nominal as not calculated yet
@ -2195,26 +2186,18 @@ void Stepper::_set_position(const int32_t &a, const int32_t &b, const int32_t &c
#if CORE_IS_XY
// corexy positioning
// these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html
count_position[A_AXIS] = a + b;
count_position[B_AXIS] = CORESIGN(a - b);
count_position[Z_AXIS] = c;
count_position.set(a + b, CORESIGN(a - b), c);
#elif CORE_IS_XZ
// corexz planning
count_position[A_AXIS] = a + c;
count_position[Y_AXIS] = b;
count_position[C_AXIS] = CORESIGN(a - c);
count_position.set(a + c, b, CORESIGN(a - c));
#elif CORE_IS_YZ
// coreyz planning
count_position[X_AXIS] = a;
count_position[B_AXIS] = b + c;
count_position[C_AXIS] = CORESIGN(b - c);
count_position.set(a, b + c, CORESIGN(b - c));
#else
// default non-h-bot planning
count_position[X_AXIS] = a;
count_position[Y_AXIS] = b;
count_position[Z_AXIS] = c;
count_position.set(a, b, c);
#endif
count_position[E_AXIS] = e;
count_position.e = e;
}
/**
@ -2290,36 +2273,22 @@ void Stepper::report_positions() {
if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
#endif
const int32_t xpos = count_position[X_AXIS],
ypos = count_position[Y_AXIS],
zpos = count_position[Z_AXIS];
const xyz_long_t pos = count_position;
#ifdef __AVR__
if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
#endif
#if CORE_IS_XY || CORE_IS_XZ || ENABLED(DELTA) || IS_SCARA
SERIAL_ECHOPGM(MSG_COUNT_A);
SERIAL_ECHOPAIR(MSG_COUNT_A, pos.x, " B:", pos.y);
#else
SERIAL_ECHOPGM(MSG_COUNT_X);
SERIAL_ECHOPAIR(MSG_COUNT_X, pos.x, " Y:", pos.y);
#endif
SERIAL_ECHO(xpos);
#if CORE_IS_XY || CORE_IS_YZ || ENABLED(DELTA) || IS_SCARA
SERIAL_ECHOPGM(" B:");
#else
SERIAL_ECHOPGM(" Y:");
#endif
SERIAL_ECHO(ypos);
#if CORE_IS_XZ || CORE_IS_YZ || ENABLED(DELTA)
SERIAL_ECHOPGM(" C:");
SERIAL_ECHOLNPAIR(" C:", pos.z);
#else
SERIAL_ECHOPGM(" Z:");
SERIAL_ECHOLNPAIR(" Z:", pos.z);
#endif
SERIAL_ECHO(zpos);
SERIAL_EOL();
}
#if ENABLED(BABYSTEPPING)

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

@ -278,9 +278,9 @@ class Stepper {
#endif
// Delta error variables for the Bresenham line tracer
static int32_t delta_error[XYZE];
static uint32_t advance_dividend[XYZE],
advance_divisor,
static xyze_long_t delta_error;
static xyze_ulong_t advance_dividend;
static uint32_t advance_divisor,
step_events_completed, // The number of step events executed in the current block
accelerate_until, // The point from where we need to stop acceleration
decelerate_after, // The point from where we need to start decelerating
@ -320,17 +320,17 @@ class Stepper {
//
// Exact steps at which an endstop was triggered
//
static volatile int32_t endstops_trigsteps[XYZ];
static xyz_long_t endstops_trigsteps;
//
// Positions of stepper motors, in step units
//
static volatile int32_t count_position[NUM_AXIS];
static xyze_long_t count_position;
//
// Current direction of stepper motors (+1 or -1)
//
static int8_t count_direction[NUM_AXIS];
static xyze_int8_t count_direction;
public:
@ -382,13 +382,11 @@ class Stepper {
// The extruder associated to the last movement
FORCE_INLINE static uint8_t movement_extruder() {
return
#if ENABLED(MIXING_EXTRUDER) || EXTRUDERS < 2
0
#else
last_moved_extruder
return (0
#if EXTRUDERS > 1 && DISABLED(MIXING_EXTRUDER)
+ last_moved_extruder
#endif
;
);
}
// Handle a triggered endstop
@ -443,8 +441,9 @@ class Stepper {
_set_position(a, b, c, e);
if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
}
static inline void set_position(const xyze_long_t &abce) { set_position(abce.a, abce.b, abce.c, abce.e); }
static inline void set_position(const AxisEnum a, const int32_t &v) {
static inline void set_axis_position(const AxisEnum a, const int32_t &v) {
planner.synchronize();
#ifdef __AVR__
@ -469,6 +468,7 @@ 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);
FORCE_INLINE static void _set_position(const abce_long_t &spos) { _set_position(spos.a, spos.b, spos.c, spos.e); }
FORCE_INLINE static uint32_t calc_timer_interval(uint32_t step_rate, uint8_t scale, uint8_t* loops) {
uint32_t timer;

175
Marlin/src/module/tool_change.cpp
View File

@ -133,8 +133,8 @@
#endif // SWITCHING_NOZZLE
inline void _line_to_current(const AxisEnum fr_axis, const float fscale=1.0f) {
planner.buffer_line(current_position, planner.settings.max_feedrate_mm_s[fr_axis] * fscale, active_extruder);
inline void _line_to_current(const AxisEnum fr_axis, const float fscale=1) {
line_to_current_position(planner.settings.max_feedrate_mm_s[fr_axis] * fscale);
}
inline void slow_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_axis, 0.5f); }
inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_axis); }
@ -150,11 +150,11 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a
inline void magnetic_parking_extruder_tool_change(const uint8_t new_tool) {
const float oldx = current_position[X_AXIS],
const float oldx = current_position.x,
grabpos = mpe_settings.parking_xpos[new_tool] + (new_tool ? mpe_settings.grab_distance : -mpe_settings.grab_distance),
offsetcompensation = (0
#if HAS_HOTEND_OFFSET
+ hotend_offset[X_AXIS][active_extruder] * mpe_settings.compensation_factor
+ hotend_offset[active_extruder].x * mpe_settings.compensation_factor
#endif
);
@ -174,7 +174,7 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a
// STEP 1
current_position[X_AXIS] = mpe_settings.parking_xpos[new_tool] + offsetcompensation;
current_position.x = mpe_settings.parking_xpos[new_tool] + offsetcompensation;
if (DEBUGGING(LEVELING)) {
DEBUG_ECHOPAIR("(1) Move extruder ", int(new_tool));
@ -186,7 +186,7 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a
// STEP 2
current_position[X_AXIS] = grabpos + offsetcompensation;
current_position.x = grabpos + offsetcompensation;
if (DEBUGGING(LEVELING)) {
DEBUG_ECHOPAIR("(2) Couple extruder ", int(new_tool));
@ -201,7 +201,7 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a
// STEP 3
current_position[X_AXIS] = mpe_settings.parking_xpos[new_tool] + offsetcompensation;
current_position.x = mpe_settings.parking_xpos[new_tool] + offsetcompensation;
if (DEBUGGING(LEVELING)) {
DEBUG_ECHOPAIR("(3) Move extruder ", int(new_tool));
DEBUG_POS(" back to new extruder ParkPos", current_position);
@ -212,7 +212,7 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a
// STEP 4
current_position[X_AXIS] = mpe_settings.parking_xpos[active_extruder] + (active_extruder == 0 ? MPE_TRAVEL_DISTANCE : -MPE_TRAVEL_DISTANCE) + offsetcompensation;
current_position.x = mpe_settings.parking_xpos[active_extruder] + (active_extruder == 0 ? MPE_TRAVEL_DISTANCE : -MPE_TRAVEL_DISTANCE) + offsetcompensation;
if (DEBUGGING(LEVELING)) {
DEBUG_ECHOPAIR("(4) Move extruder ", int(new_tool));
DEBUG_POS(" close to old extruder ParkPos", current_position);
@ -223,7 +223,7 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a
// STEP 5
current_position[X_AXIS] = mpe_settings.parking_xpos[active_extruder] + offsetcompensation;
current_position.x = mpe_settings.parking_xpos[active_extruder] + offsetcompensation;
if (DEBUGGING(LEVELING)) {
DEBUG_ECHOPAIR("(5) Park extruder ", int(new_tool));
@ -235,7 +235,7 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a
// STEP 6
current_position[X_AXIS] = oldx;
current_position.x = oldx;
if (DEBUGGING(LEVELING)) {
DEBUG_ECHOPAIR("(6) Move extruder ", int(new_tool));
@ -275,12 +275,12 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a
constexpr float parkingposx[] = PARKING_EXTRUDER_PARKING_X;
#if HAS_HOTEND_OFFSET
const float x_offset = hotend_offset[X_AXIS][active_extruder];
const float x_offset = hotend_offset[active_extruder].x;
#else
constexpr float x_offset = 0;
#endif
const float midpos = (parkingposx[0] + parkingposx[1]) * 0.5 + x_offset,
const float midpos = (parkingposx[0] + parkingposx[1]) * 0.5f + x_offset,
grabpos = parkingposx[new_tool] + (new_tool ? PARKING_EXTRUDER_GRAB_DISTANCE : -(PARKING_EXTRUDER_GRAB_DISTANCE)) + x_offset;
/**
@ -296,7 +296,7 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a
if (DEBUGGING(LEVELING)) DEBUG_POS("Start PE Tool-Change", current_position);
current_position[X_AXIS] = parkingposx[active_extruder] + x_offset;
current_position.x = parkingposx[active_extruder] + x_offset;
if (DEBUGGING(LEVELING)) {
DEBUG_ECHOLNPAIR("(1) Park extruder ", int(active_extruder));
DEBUG_POS("Moving ParkPos", current_position);
@ -311,7 +311,7 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a
// STEP 3
current_position[X_AXIS] += active_extruder ? -10 : 10; // move 10mm away from parked extruder
current_position.x += active_extruder ? -10 : 10; // move 10mm away from parked extruder
if (DEBUGGING(LEVELING)) {
DEBUG_ECHOLNPGM("(3) Move near new extruder");
DEBUG_POS("Move away from parked extruder", current_position);
@ -329,10 +329,10 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a
// STEP 5
current_position[X_AXIS] = grabpos + (new_tool ? -10 : 10);
current_position.x = grabpos + (new_tool ? -10 : 10);
fast_line_to_current(X_AXIS);
current_position[X_AXIS] = grabpos;
current_position.x = grabpos;
if (DEBUGGING(LEVELING)) {
planner.synchronize();
DEBUG_POS("(5) Unpark extruder", current_position);
@ -341,9 +341,9 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a
// STEP 6
current_position[X_AXIS] = midpos
current_position.x = midpos
#if HAS_HOTEND_OFFSET
- hotend_offset[X_AXIS][new_tool]
- hotend_offset[new_tool].x
#endif
;
if (DEBUGGING(LEVELING)) {
@ -388,14 +388,14 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a
if (DEBUGGING(LEVELING)) DEBUG_POS("Start ST Tool-Change", current_position);
current_position[X_AXIS] = placexpos;
current_position.x = placexpos;
if (DEBUGGING(LEVELING)) {
DEBUG_ECHOLNPAIR("(1) Place old tool ", int(active_extruder));
DEBUG_POS("Move X SwitchPos", current_position);
}
fast_line_to_current(X_AXIS);
current_position[Y_AXIS] = SWITCHING_TOOLHEAD_Y_POS - (SWITCHING_TOOLHEAD_Y_SECURITY);
current_position.y = SWITCHING_TOOLHEAD_Y_POS - (SWITCHING_TOOLHEAD_Y_SECURITY);
if (DEBUGGING(LEVELING)) {
planner.synchronize();
DEBUG_POS("Move Y SwitchPos + Security", current_position);
@ -409,7 +409,7 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a
MOVE_SERVO(SWITCHING_TOOLHEAD_SERVO_NR, angles[1]);
safe_delay(500);
current_position[Y_AXIS] = SWITCHING_TOOLHEAD_Y_POS;
current_position.y = SWITCHING_TOOLHEAD_Y_POS;
if (DEBUGGING(LEVELING)) DEBUG_POS("Move Y SwitchPos", current_position);
slow_line_to_current(Y_AXIS);
@ -417,13 +417,13 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a
planner.synchronize();
safe_delay(200);
current_position[Y_AXIS] -= SWITCHING_TOOLHEAD_Y_CLEAR;
current_position.y -= SWITCHING_TOOLHEAD_Y_CLEAR;
if (DEBUGGING(LEVELING)) DEBUG_POS("Move back Y clear", current_position);
fast_line_to_current(Y_AXIS); // move away from docked toolhead
// 3. Move to the new toolhead
current_position[X_AXIS] = grabxpos;
current_position.x = grabxpos;
if (DEBUGGING(LEVELING)) {
planner.synchronize();
DEBUG_ECHOLNPGM("(3) Move to new toolhead position");
@ -431,7 +431,7 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a
}
fast_line_to_current(X_AXIS);
current_position[Y_AXIS] = SWITCHING_TOOLHEAD_Y_POS - (SWITCHING_TOOLHEAD_Y_SECURITY);
current_position.y = SWITCHING_TOOLHEAD_Y_POS - (SWITCHING_TOOLHEAD_Y_SECURITY);
if (DEBUGGING(LEVELING)) {
planner.synchronize();
DEBUG_POS("Move Y SwitchPos + Security", current_position);
@ -440,7 +440,7 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a
// 4. Grab and lock the new toolhead
current_position[Y_AXIS] = SWITCHING_TOOLHEAD_Y_POS;
current_position.y = SWITCHING_TOOLHEAD_Y_POS;
if (DEBUGGING(LEVELING)) {
planner.synchronize();
DEBUG_ECHOLNPGM("(4) Grab and lock new toolhead");
@ -454,7 +454,7 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a
MOVE_SERVO(SWITCHING_TOOLHEAD_SERVO_NR, angles[0]);
safe_delay(500);
current_position[Y_AXIS] -= SWITCHING_TOOLHEAD_Y_CLEAR;
current_position.y -= SWITCHING_TOOLHEAD_Y_CLEAR;
if (DEBUGGING(LEVELING)) DEBUG_POS("Move back Y clear", current_position);
fast_line_to_current(Y_AXIS); // Move away from docked toolhead
planner.synchronize(); // Always sync the final move
@ -486,33 +486,33 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a
// 1. Move to switch position current toolhead
current_position[Y_AXIS] = SWITCHING_TOOLHEAD_Y_POS + SWITCHING_TOOLHEAD_Y_CLEAR;
current_position.y = SWITCHING_TOOLHEAD_Y_POS + SWITCHING_TOOLHEAD_Y_CLEAR;
if (DEBUGGING(LEVELING)) {
SERIAL_ECHOLNPAIR("(1) Place old tool ", int(active_extruder));
DEBUG_POS("Move Y SwitchPos + Security", current_position);
}
fast_line_to_current(Y_AXIS);
current_position[X_AXIS] = placexclear;
current_position.x = placexclear;
if (DEBUGGING(LEVELING)) {
planner.synchronize();
DEBUG_POS("Move X SwitchPos + Security", current_position);
}
fast_line_to_current(X_AXIS);
current_position[Y_AXIS] = SWITCHING_TOOLHEAD_Y_POS;
current_position.y = SWITCHING_TOOLHEAD_Y_POS;
if (DEBUGGING(LEVELING)) {
planner.synchronize();
DEBUG_POS("Move Y SwitchPos", current_position);
}
fast_line_to_current(Y_AXIS);
current_position[X_AXIS] = placexpos;
current_position.x = placexpos;
if (DEBUGGING(LEVELING)) {
planner.synchronize();
DEBUG_POS("Move X SwitchPos", current_position);
}
planner.buffer_line(current_position, (planner.settings.max_feedrate_mm_s[X_AXIS] * 0.25), active_extruder);
line_to_current_position(planner.settings.max_feedrate_mm_s[X_AXIS] * 0.25f);
// 2. Release and place toolhead in the dock
@ -521,16 +521,16 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a
DEBUG_ECHOLNPGM("(2) Release and Place Toolhead");
}
current_position[Y_AXIS] = SWITCHING_TOOLHEAD_Y_POS + SWITCHING_TOOLHEAD_Y_RELEASE;
current_position.y = SWITCHING_TOOLHEAD_Y_POS + SWITCHING_TOOLHEAD_Y_RELEASE;
if (DEBUGGING(LEVELING)) DEBUG_POS("Move Y SwitchPos + Release", current_position);
planner.buffer_line(current_position, (planner.settings.max_feedrate_mm_s[Y_AXIS] * 0.1), active_extruder);
line_to_current_position(planner.settings.max_feedrate_mm_s[Y_AXIS] * 0.1f);
current_position[Y_AXIS] = SWITCHING_TOOLHEAD_Y_POS + SWITCHING_TOOLHEAD_Y_SECURITY;
current_position.y = SWITCHING_TOOLHEAD_Y_POS + SWITCHING_TOOLHEAD_Y_SECURITY;
if (DEBUGGING(LEVELING)) {
planner.synchronize();
DEBUG_POS("Move Y SwitchPos + Security", current_position);
}
planner.buffer_line(current_position, (planner.settings.max_feedrate_mm_s[Y_AXIS]), active_extruder);
line_to_current_position(planner.settings.max_feedrate_mm_s[Y_AXIS]);
// 3. Move to new toolhead position
@ -539,7 +539,7 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a
DEBUG_ECHOLNPGM("(3) Move to new toolhead position");
}
current_position[X_AXIS] = grabxpos;
current_position.x = grabxpos;
if (DEBUGGING(LEVELING)) DEBUG_POS("Move to new toolhead X", current_position);
fast_line_to_current(X_AXIS);
@ -550,11 +550,11 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a
DEBUG_ECHOLNPGM("(4) Grab new toolhead, move to security position");
}
current_position[Y_AXIS] = SWITCHING_TOOLHEAD_Y_POS + SWITCHING_TOOLHEAD_Y_RELEASE;
current_position.y = SWITCHING_TOOLHEAD_Y_POS + SWITCHING_TOOLHEAD_Y_RELEASE;
if (DEBUGGING(LEVELING)) DEBUG_POS("Move Y SwitchPos + Release", current_position);
planner.buffer_line(current_position, (planner.settings.max_feedrate_mm_s[Y_AXIS]), active_extruder);
line_to_current_position(planner.settings.max_feedrate_mm_s[Y_AXIS]);
current_position[Y_AXIS] = SWITCHING_TOOLHEAD_Y_POS;
current_position.y = SWITCHING_TOOLHEAD_Y_POS;
if (DEBUGGING(LEVELING)) {
planner.synchronize();
DEBUG_POS("Move Y SwitchPos", current_position);
@ -563,11 +563,11 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a
#if ENABLED(PRIME_BEFORE_REMOVE) && (SWITCHING_TOOLHEAD_PRIME_MM || SWITCHING_TOOLHEAD_RETRACT_MM)
#if SWITCHING_TOOLHEAD_PRIME_MM
current_position[E_AXIS] += SWITCHING_TOOLHEAD_PRIME_MM;
current_position.e += SWITCHING_TOOLHEAD_PRIME_MM;
planner.buffer_line(current_position, MMM_TO_MMS(SWITCHING_TOOLHEAD_PRIME_FEEDRATE), new_tool);
#endif
#if SWITCHING_TOOLHEAD_RETRACT_MM
current_position[E_AXIS] -= SWITCHING_TOOLHEAD_RETRACT_MM;
current_position.e -= SWITCHING_TOOLHEAD_RETRACT_MM;
planner.buffer_line(current_position, MMM_TO_MMS(SWITCHING_TOOLHEAD_RETRACT_FEEDRATE), new_tool);
#endif
#else
@ -575,13 +575,13 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a
safe_delay(100); // Give switch time to settle
#endif
current_position[X_AXIS] = grabxclear;
current_position.x = grabxclear;
if (DEBUGGING(LEVELING)) DEBUG_POS("Move to new toolhead X + Security", current_position);
_line_to_current(X_AXIS, 0.1f);
planner.synchronize();
safe_delay(100); // Give switch time to settle
current_position[Y_AXIS] += SWITCHING_TOOLHEAD_Y_CLEAR;
current_position.y += SWITCHING_TOOLHEAD_Y_CLEAR;
if (DEBUGGING(LEVELING)) DEBUG_POS("Move back Y clear", current_position);
fast_line_to_current(Y_AXIS); // move away from docked toolhead
planner.synchronize(); // Always sync last tool-change move
@ -601,6 +601,7 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a
constexpr float toolheadposx[] = SWITCHING_TOOLHEAD_X_POS;
const float placexpos = toolheadposx[active_extruder],
grabxpos = toolheadposx[new_tool];
const xyz_pos_t &hoffs = hotend_offset[active_extruder];
/**
* 1. Raise Z-Axis to give enough clearance
@ -618,7 +619,7 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a
// 1. Raise Z-Axis to give enough clearance
current_position[Z_AXIS] += SWITCHING_TOOLHEAD_Z_HOP;
current_position.z += SWITCHING_TOOLHEAD_Z_HOP;
if (DEBUGGING(LEVELING)) DEBUG_POS("(1) Raise Z-Axis ", current_position);
fast_line_to_current(Z_AXIS);
@ -629,8 +630,8 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a
SERIAL_ECHOLNPAIR("(2) Move near active extruder parking", active_extruder);
DEBUG_POS("Moving ParkPos", current_position);
}
current_position[X_AXIS] = hotend_offset[X_AXIS][active_extruder] + placexpos;
current_position[Y_AXIS] = hotend_offset[Y_AXIS][active_extruder] + SWITCHING_TOOLHEAD_Y_POS + SWITCHING_TOOLHEAD_Y_CLEAR;
current_position.set(hoffs.x + placexpos,
hoffs.y + SWITCHING_TOOLHEAD_Y_POS + SWITCHING_TOOLHEAD_Y_CLEAR);
fast_line_to_current(X_AXIS);
// 3. Move gently to park position of active extruder
@ -641,7 +642,7 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a
DEBUG_POS("Moving ParkPos", current_position);
}
current_position[Y_AXIS] -= SWITCHING_TOOLHEAD_Y_CLEAR;
current_position.y -= SWITCHING_TOOLHEAD_Y_CLEAR;
slow_line_to_current(Y_AXIS);
// 4. Disengage magnetic field, wait for delay
@ -657,16 +658,15 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a
DEBUG_POS("Moving ParkPos", current_position);
}
current_position[Y_AXIS] += SWITCHING_TOOLHEAD_Y_CLEAR;
current_position.y += SWITCHING_TOOLHEAD_Y_CLEAR;
slow_line_to_current(Y_AXIS);
current_position[X_AXIS] = hotend_offset[X_AXIS][active_extruder] + grabxpos;
current_position[Y_AXIS] = hotend_offset[Y_AXIS][active_extruder] + SWITCHING_TOOLHEAD_Y_POS + SWITCHING_TOOLHEAD_Y_CLEAR;
current_position.set(hoffs.x + grabxpos,
hoffs.y + SWITCHING_TOOLHEAD_Y_POS + SWITCHING_TOOLHEAD_Y_CLEAR);
fast_line_to_current(X_AXIS);
// 6. Move gently to park position of new extruder
current_position[Y_AXIS] -= SWITCHING_TOOLHEAD_Y_CLEAR;
current_position.y -= SWITCHING_TOOLHEAD_Y_CLEAR;
if (DEBUGGING(LEVELING)) {
planner.synchronize();
DEBUG_ECHOLNPGM("(6) Move near new extruder");
@ -681,7 +681,7 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a
// 8. Unpark extruder
current_position[Y_AXIS] += SWITCHING_TOOLHEAD_Y_CLEAR;
current_position.y += SWITCHING_TOOLHEAD_Y_CLEAR;
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("(8) Unpark extruder");
slow_line_to_current(X_AXIS);
planner.synchronize(); // Always sync the final move
@ -689,7 +689,7 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a
// 9. Apply Z hotend offset to current position
if (DEBUGGING(LEVELING)) DEBUG_POS("(9) Applying Z-offset", current_position);
current_position[Z_AXIS] += hotend_offset[Z_AXIS][active_extruder] - hotend_offset[Z_AXIS][new_tool];
current_position.z += hoffs.z - hotend_offset[new_tool].z;
if (DEBUGGING(LEVELING)) DEBUG_POS("EMST Tool-Change done.", current_position);
}
@ -719,14 +719,15 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a
const float xhome = x_home_pos(active_extruder);
if (dual_x_carriage_mode == DXC_AUTO_PARK_MODE
&& IsRunning()
&& (delayed_move_time || current_position[X_AXIS] != xhome) && ! no_move
&& IsRunning() && !no_move
&& (delayed_move_time || current_position.x != xhome)
) {
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("MoveX to ", xhome);
// Park old head
planner.buffer_line(xhome, current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], planner.settings.max_feedrate_mm_s[X_AXIS], active_extruder);
current_position.x = xhome;
line_to_current_position(planner.settings.max_feedrate_mm_s[X_AXIS]);
planner.synchronize();
}
@ -741,13 +742,13 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a
switch (dual_x_carriage_mode) {
case DXC_FULL_CONTROL_MODE:
// New current position is the position of the activated extruder
current_position[X_AXIS] = inactive_extruder_x_pos;
current_position.x = inactive_extruder_x_pos;
// Save the inactive extruder's position (from the old current_position)
inactive_extruder_x_pos = destination[X_AXIS];
inactive_extruder_x_pos = destination.x;
break;
case DXC_AUTO_PARK_MODE:
// record current raised toolhead position for use by unpark
COPY(raised_parked_position, current_position);
raised_parked_position = current_position;
active_extruder_parked = true;
delayed_move_time = 0;
break;
@ -852,7 +853,7 @@ void tool_change(const uint8_t new_tool, bool no_move/*=false*/) {
#if ENABLED(ADVANCED_PAUSE_FEATURE)
do_pause_e_move(-toolchange_settings.swap_length, MMM_TO_MMS(toolchange_settings.retract_speed));
#else
current_position[E_AXIS] -= toolchange_settings.swap_length / planner.e_factor[old_tool];
current_position.e -= toolchange_settings.swap_length / planner.e_factor[old_tool];
planner.buffer_line(current_position, MMM_TO_MMS(toolchange_settings.retract_speed), old_tool);
planner.synchronize();
#endif
@ -886,19 +887,18 @@ void tool_change(const uint8_t new_tool, bool no_move/*=false*/) {
#endif
#endif
set_destination_from_current();
destination = current_position;
#if DISABLED(SWITCHING_NOZZLE)
if (can_move_away) {
// Do a small lift to avoid the workpiece in the move back (below)
current_position[Z_AXIS] += toolchange_settings.z_raise;
current_position.z += toolchange_settings.z_raise;
#if HAS_SOFTWARE_ENDSTOPS
NOMORE(current_position[Z_AXIS], soft_endstop[Z_AXIS].max);
NOMORE(current_position.z, soft_endstop.max.z);
#endif
fast_line_to_current(Z_AXIS);
#if ENABLED(TOOLCHANGE_PARK)
current_position[X_AXIS] = toolchange_settings.change_point.x;
current_position[Y_AXIS] = toolchange_settings.change_point.y;
current_position = toolchange_settings.change_point;
#endif
planner.buffer_line(current_position, feedrate_mm_s, old_tool);
planner.synchronize();
@ -906,15 +906,12 @@ void tool_change(const uint8_t new_tool, bool no_move/*=false*/) {
#endif
#if HAS_HOTEND_OFFSET
xyz_pos_t diff = hotend_offset[new_tool];
#if ENABLED(DUAL_X_CARRIAGE)
constexpr float xdiff = 0;
#else
const float xdiff = hotend_offset[X_AXIS][new_tool] - hotend_offset[X_AXIS][old_tool];
diff.x = 0;
#endif
const float ydiff = hotend_offset[Y_AXIS][new_tool] - hotend_offset[Y_AXIS][old_tool],
zdiff = hotend_offset[Z_AXIS][new_tool] - hotend_offset[Z_AXIS][old_tool];
#else
constexpr float xdiff = 0, ydiff = 0, zdiff = 0;
constexpr xyz_pos_t diff{0};
#endif
#if ENABLED(DUAL_X_CARRIAGE)
@ -932,30 +929,28 @@ void tool_change(const uint8_t new_tool, bool no_move/*=false*/) {
#elif ENABLED(SWITCHING_NOZZLE) && !SWITCHING_NOZZLE_TWO_SERVOS // Switching Nozzle (single servo)
// Raise by a configured distance to avoid workpiece, except with
// SWITCHING_NOZZLE_TWO_SERVOS, as both nozzles will lift instead.
current_position[Z_AXIS] += _MAX(-zdiff, 0.0) + toolchange_settings.z_raise;
current_position.z += _MAX(-zdiff, 0.0) + toolchange_settings.z_raise;
#if HAS_SOFTWARE_ENDSTOPS
NOMORE(current_position[Z_AXIS], soft_endstop[Z_AXIS].max);
NOMORE(current_position.z, soft_endstop.max.z);
#endif
if (!no_move) fast_line_to_current(Z_AXIS);
move_nozzle_servo(new_tool);
#endif
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("Offset Tool XY by { ", xdiff, ", ", ydiff, ", ", zdiff, " }");
#if DISABLED(DUAL_X_CARRIAGE)
active_extruder = new_tool; // Set the new active extruder
#endif
// The newly-selected extruder XY is actually at...
current_position[X_AXIS] += xdiff;
current_position[Y_AXIS] += ydiff;
current_position[Z_AXIS] += zdiff;
// Set the new active extruder if not already done in tool specific function above
active_extruder = new_tool;
// The newly-selected extruder XYZ is actually at...
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("Offset Tool XY by { ", diff.x, ", ", diff.y, ", ", diff.z, " }");
current_position += diff;
// Tell the planner the new "current position"
sync_plan_position();
#if ENABLED(DELTA)
//LOOP_XYZ(i) update_software_endstops(i); // or modify the constrain function
const bool safe_to_move = current_position[Z_AXIS] < delta_clip_start_height - 1;
const bool safe_to_move = current_position.z < delta_clip_start_height - 1;
#else
constexpr bool safe_to_move = true;
#endif
@ -985,21 +980,21 @@ void tool_change(const uint8_t new_tool, bool no_move/*=false*/) {
do_pause_e_move(toolchange_settings.swap_length, MMM_TO_MMS(toolchange_settings.prime_speed));
do_pause_e_move(toolchange_settings.extra_prime, ADVANCED_PAUSE_PURGE_FEEDRATE);
#else
current_position[E_AXIS] += toolchange_settings.swap_length / planner.e_factor[new_tool];
current_position.e += toolchange_settings.swap_length / planner.e_factor[new_tool];
planner.buffer_line(current_position, MMM_TO_MMS(toolchange_settings.prime_speed), new_tool);
current_position[E_AXIS] += toolchange_settings.extra_prime / planner.e_factor[new_tool];
current_position.e += toolchange_settings.extra_prime / planner.e_factor[new_tool];
planner.buffer_line(current_position, MMM_TO_MMS(toolchange_settings.prime_speed * 0.2f), new_tool);
#endif
planner.synchronize();
planner.set_e_position_mm((destination[E_AXIS] = current_position[E_AXIS] = current_position[E_AXIS] - (TOOLCHANGE_FIL_EXTRA_PRIME)));
planner.set_e_position_mm((destination.e = current_position.e = current_position.e - (TOOLCHANGE_FIL_EXTRA_PRIME)));
}
#endif
// Prevent a move outside physical bounds
#if ENABLED(MAGNETIC_SWITCHING_TOOLHEAD)
// If the original position is within tool store area, go to X origin at once
if (destination[Y_AXIS] < SWITCHING_TOOLHEAD_Y_POS + SWITCHING_TOOLHEAD_Y_CLEAR) {
current_position[X_AXIS] = 0;
if (destination.y < SWITCHING_TOOLHEAD_Y_POS + SWITCHING_TOOLHEAD_Y_CLEAR) {
current_position.x = 0;
planner.buffer_line(current_position, planner.settings.max_feedrate_mm_s[X_AXIS], new_tool);
planner.synchronize();
}
@ -1012,7 +1007,7 @@ void tool_change(const uint8_t new_tool, bool no_move/*=false*/) {
#if ENABLED(TOOLCHANGE_NO_RETURN)
// Just move back down
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Move back Z only");
do_blocking_move_to_z(destination[Z_AXIS], planner.settings.max_feedrate_mm_s[Z_AXIS]);
do_blocking_move_to_z(destination.z, planner.settings.max_feedrate_mm_s[Z_AXIS]);
#else
// Move back to the original (or adjusted) position
if (DEBUGGING(LEVELING)) DEBUG_POS("Move back", destination);
@ -1028,7 +1023,7 @@ void tool_change(const uint8_t new_tool, bool no_move/*=false*/) {
#if ENABLED(SWITCHING_NOZZLE)
else {
// Move back down. (Including when the new tool is higher.)
do_blocking_move_to_z(destination[Z_AXIS], planner.settings.max_feedrate_mm_s[Z_AXIS]);
do_blocking_move_to_z(destination.z, planner.settings.max_feedrate_mm_s[Z_AXIS]);
}
#endif

3
Marlin/src/module/tool_change.h
View File

@ -22,6 +22,7 @@
#pragma once
#include "../inc/MarlinConfigPre.h"
#include "../core/types.h"
#if EXTRUDERS > 1
@ -31,7 +32,7 @@
int16_t prime_speed, retract_speed;
#endif
#if ENABLED(TOOLCHANGE_PARK)
struct { float x, y; } change_point;
xy_pos_t change_point;
#endif
float z_raise;
} toolchange_settings_t;