MPCNC/Firmware2
1
0
Fork 0
Code Issues 1 Pull Requests Releases Wiki Activity

Overhaul of the planner (#11578)

Browse Source 
- Move FWRETRACT to the planner
- Combine leveling, skew, etc. in a single modifier method
- Have kinematic and non-kinematic moves call one planner method
This commit is contained in:
Thomas Moore 2018-09-16 22:24:15 -04:00 committed by Scott Lahteine
parent 8323a08642
commit c437bb08f1
39 changed files with 655 additions and 597 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
Marlin/src/Marlin.cpp
View File

@ -275,7 +275,7 @@ void quickstop_stepper() {
planner.quick_stop();
planner.synchronize();
set_current_from_steppers_for_axis(ALL_AXES);
SYNC_PLAN_POSITION_KINEMATIC();
sync_plan_position();
}
void enable_all_steppers() {
@ -465,7 +465,7 @@ void manage_inactivity(const bool ignore_stepper_queue/*=false*/) {
const float olde = current_position[E_AXIS];
current_position[E_AXIS] += EXTRUDER_RUNOUT_EXTRUDE;
planner.buffer_line_kinematic(current_position, MMM_TO_MMS(EXTRUDER_RUNOUT_SPEED), active_extruder);
planner.buffer_line(current_position, MMM_TO_MMS(EXTRUDER_RUNOUT_SPEED), active_extruder);
current_position[E_AXIS] = olde;
planner.set_e_position_mm(olde);
planner.synchronize();
@ -766,7 +766,7 @@ void setup() {
#endif
// Vital to init stepper/planner equivalent for current_position
SYNC_PLAN_POSITION_KINEMATIC();
sync_plan_position();
thermalManager.init(); // Initialize temperature loop

3
Marlin/src/config/examples/delta/FLSUN/auto_calibrate/Configuration.h
View File

@ -539,9 +539,6 @@
// and processor overload (too many expensive sqrt calls).
#define DELTA_SEGMENTS_PER_SECOND 160
// Convert feedrates to apply to the Effector instead of the Carriages
#define DELTA_FEEDRATE_SCALING
// After homing move down to a height where XY movement is unconstrained
//#define DELTA_HOME_TO_SAFE_ZONE

3
Marlin/src/config/examples/delta/FLSUN/kossel/Configuration.h
View File

@ -539,9 +539,6 @@
// and processor overload (too many expensive sqrt calls).
#define DELTA_SEGMENTS_PER_SECOND 160
// Convert feedrates to apply to the Effector instead of the Carriages
#define DELTA_FEEDRATE_SCALING
// After homing move down to a height where XY movement is unconstrained
//#define DELTA_HOME_TO_SAFE_ZONE

3
Marlin/src/config/examples/delta/FLSUN/kossel_mini/Configuration.h
View File

@ -539,9 +539,6 @@
// and processor overload (too many expensive sqrt calls).
#define DELTA_SEGMENTS_PER_SECOND 160
// Convert feedrates to apply to the Effector instead of the Carriages
#define DELTA_FEEDRATE_SCALING
// After homing move down to a height where XY movement is unconstrained
//#define DELTA_HOME_TO_SAFE_ZONE

3
Marlin/src/config/examples/delta/Hatchbox_Alpha/Configuration.h
View File

@ -544,9 +544,6 @@
// and processor overload (too many expensive sqrt calls).
#define DELTA_SEGMENTS_PER_SECOND 200
// Convert feedrates to apply to the Effector instead of the Carriages
#define DELTA_FEEDRATE_SCALING
// After homing move down to a height where XY movement is unconstrained
//#define DELTA_HOME_TO_SAFE_ZONE

3
Marlin/src/config/examples/delta/generic/Configuration.h
View File

@ -529,9 +529,6 @@
// and processor overload (too many expensive sqrt calls).
#define DELTA_SEGMENTS_PER_SECOND 200
// Convert feedrates to apply to the Effector instead of the Carriages
#define DELTA_FEEDRATE_SCALING
// After homing move down to a height where XY movement is unconstrained
//#define DELTA_HOME_TO_SAFE_ZONE

3
Marlin/src/config/examples/delta/kossel_mini/Configuration.h
View File

@ -529,9 +529,6 @@
// and processor overload (too many expensive sqrt calls).
#define DELTA_SEGMENTS_PER_SECOND 200
// Convert feedrates to apply to the Effector instead of the Carriages
#define DELTA_FEEDRATE_SCALING
// After homing move down to a height where XY movement is unconstrained
//#define DELTA_HOME_TO_SAFE_ZONE

3
Marlin/src/config/examples/delta/kossel_pro/Configuration.h
View File

@ -515,9 +515,6 @@
// and processor overload (too many expensive sqrt calls).
#define DELTA_SEGMENTS_PER_SECOND 160
// Convert feedrates to apply to the Effector instead of the Carriages
#define DELTA_FEEDRATE_SCALING
// After homing move down to a height where XY movement is unconstrained
//#define DELTA_HOME_TO_SAFE_ZONE

3
Marlin/src/config/examples/delta/kossel_xl/Configuration.h
View File

@ -533,9 +533,6 @@
// and processor overload (too many expensive sqrt calls).
#define DELTA_SEGMENTS_PER_SECOND 160
// Convert feedrates to apply to the Effector instead of the Carriages
#define DELTA_FEEDRATE_SCALING
// After homing move down to a height where XY movement is unconstrained
//#define DELTA_HOME_TO_SAFE_ZONE

87
Marlin/src/feature/bedlevel/bedlevel.cpp
View File

@ -25,15 +25,12 @@
#if HAS_LEVELING
#include "bedlevel.h"
#include "../../module/planner.h"
#if ENABLED(MESH_BED_LEVELING) || ENABLED(PROBE_MANUALLY)
#include "../../module/motion.h"
#endif
#if PLANNER_LEVELING
#include "../../module/planner.h"
#endif
#if ENABLED(PROBE_MANUALLY)
bool g29_in_progress = false;
#endif
@ -79,74 +76,24 @@ void set_bed_leveling_enabled(const bool enable/*=true*/) {
planner.synchronize();
#if ENABLED(MESH_BED_LEVELING)
#if ENABLED(AUTO_BED_LEVELING_BILINEAR)
// Force bilinear_z_offset to re-calculate next time
const float reset[XYZ] = { -9999.999, -9999.999, 0 };
(void)bilinear_z_offset(reset);
#endif
if (!enable)
planner.apply_leveling(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
if (planner.leveling_active) { // leveling from on to off
// change unleveled current_position to physical current_position without moving steppers.
planner.apply_leveling(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
planner.leveling_active = false; // disable only AFTER calling apply_leveling
}
else { // leveling from off to on
planner.leveling_active = true; // enable BEFORE calling unapply_leveling, otherwise ignored
// change physical current_position to unleveled current_position without moving steppers.
planner.unapply_leveling(current_position);
}
const bool enabling = enable && leveling_is_valid();
planner.leveling_active = enabling;
if (enabling) planner.unapply_leveling(current_position);
#elif ENABLED(AUTO_BED_LEVELING_UBL)
#if PLANNER_LEVELING
if (planner.leveling_active) { // leveling from on to off
// change unleveled current_position to physical current_position without moving steppers.
planner.apply_leveling(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
planner.leveling_active = false; // disable only AFTER calling apply_leveling
}
else { // leveling from off to on
planner.leveling_active = true; // enable BEFORE calling unapply_leveling, otherwise ignored
// change physical current_position to unleveled current_position without moving steppers.
planner.unapply_leveling(current_position);
}
#else
// UBL equivalents for apply/unapply_leveling
#if ENABLED(SKEW_CORRECTION)
float pos[XYZ] = { current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] };
planner.skew(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS]);
#else
const float (&pos)[XYZE] = current_position;
#endif
if (planner.leveling_active) {
current_position[Z_AXIS] += ubl.get_z_correction(pos[X_AXIS], pos[Y_AXIS]);
planner.leveling_active = false;
}
else {
planner.leveling_active = true;
current_position[Z_AXIS] -= ubl.get_z_correction(pos[X_AXIS], pos[Y_AXIS]);
}
#endif
#else // OLDSCHOOL_ABL
#if ENABLED(AUTO_BED_LEVELING_BILINEAR)
// Force bilinear_z_offset to re-calculate next time
const float reset[XYZ] = { -9999.999, -9999.999, 0 };
(void)bilinear_z_offset(reset);
#endif
// Enable or disable leveling compensation in the planner
planner.leveling_active = enable;
if (!enable)
// When disabling just get the current position from the steppers.
// This will yield the smallest error when first converted back to steps.
set_current_from_steppers_for_axis(
#if ABL_PLANAR
ALL_AXES
#else
Z_AXIS
#endif
);
else
// When enabling, remove compensation from the current position,
// so compensation will give the right stepper counts.
planner.unapply_leveling(current_position);
SYNC_PLAN_POSITION_KINEMATIC();
#endif // OLDSCHOOL_ABL
sync_plan_position();
}
}

73
Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp
View File

@ -49,12 +49,11 @@
* as possible to determine if this is the case. If this move is within the same cell, we will
* just do the required Z-Height correction, call the Planner's buffer_line() routine, and leave
*/
#if ENABLED(SKEW_CORRECTION)
// For skew correction just adjust the destination point and we're done
#if HAS_POSITION_MODIFIERS
float start[XYZE] = { current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS] },
end[XYZE] = { destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS] };
planner.skew(start[X_AXIS], start[Y_AXIS], start[Z_AXIS]);
planner.skew(end[X_AXIS], end[Y_AXIS], end[Z_AXIS]);
planner.apply_modifiers(start);
planner.apply_modifiers(end);
#else
const float (&start)[XYZE] = current_position,
(&end)[XYZE] = destination;
@ -364,47 +363,6 @@
#else // UBL_SEGMENTED
#if IS_SCARA // scale the feed rate from mm/s to degrees/s
static float scara_feed_factor, scara_oldA, scara_oldB;
#endif
// We don't want additional apply_leveling() performed by regular buffer_line or buffer_line_kinematic,
// so we call buffer_segment directly here. Per-segmented leveling and kinematics performed first.
inline void _O2 ubl_buffer_segment_raw(const float (&in_raw)[XYZE], const float &fr) {
#if ENABLED(SKEW_CORRECTION)
float raw[XYZE] = { in_raw[X_AXIS], in_raw[Y_AXIS], in_raw[Z_AXIS] };
planner.skew(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]);
#else
const float (&raw)[XYZE] = in_raw;
#endif
#if ENABLED(DELTA) // apply delta inverse_kinematics
DELTA_IK(raw);
planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], in_raw[E_AXIS], fr, active_extruder);
#elif IS_SCARA // apply scara inverse_kinematics (should be changed to save raw->logical->raw)
inverse_kinematics(raw); // this writes delta[ABC] from raw[XYZE]
// should move the feedrate scaling to scara inverse_kinematics
const float adiff = ABS(delta[A_AXIS] - scara_oldA),
bdiff = ABS(delta[B_AXIS] - scara_oldB);
scara_oldA = delta[A_AXIS];
scara_oldB = delta[B_AXIS];
float s_feedrate = MAX(adiff, bdiff) * scara_feed_factor;
planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], in_raw[E_AXIS], s_feedrate, active_extruder);
#else // CARTESIAN
planner.buffer_segment(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], in_raw[E_AXIS], fr, active_extruder);
#endif
}
#if IS_SCARA
#define DELTA_SEGMENT_MIN_LENGTH 0.25 // SCARA minimum segment size is 0.25mm
#elif ENABLED(DELTA)
@ -449,10 +407,9 @@
NOLESS(segments, 1U); // must have at least one segment
const float inv_segments = 1.0f / segments; // divide once, multiply thereafter
#if IS_SCARA // scale the feed rate from mm/s to degrees/s
scara_feed_factor = cartesian_xy_mm * inv_segments * feedrate;
scara_oldA = planner.get_axis_position_degrees(A_AXIS);
scara_oldB = planner.get_axis_position_degrees(B_AXIS);
const float segment_xyz_mm = HYPOT(cartesian_xy_mm, total[Z_AXIS]) * inv_segments; // length of each segment
#if ENABLED(SCARA_FEEDRATE_SCALING)
const float inv_duration = feedrate / segment_xyz_mm;
#endif
const float diff[XYZE] = {
@ -476,9 +433,17 @@
if (!planner.leveling_active || !planner.leveling_active_at_z(rtarget[Z_AXIS])) { // no mesh leveling
while (--segments) {
LOOP_XYZE(i) raw[i] += diff[i];
ubl_buffer_segment_raw(raw, feedrate);
planner.buffer_line(raw, feedrate, active_extruder, segment_xyz_mm
#if ENABLED(SCARA_FEEDRATE_SCALING)
, inv_duration
#endif
);
}
ubl_buffer_segment_raw(rtarget, feedrate);
planner.buffer_line(rtarget, feedrate, active_extruder, segment_xyz_mm
#if ENABLED(SCARA_FEEDRATE_SCALING)
, inv_duration
#endif
);
return false; // moved but did not set_current_from_destination();
}
@ -554,7 +519,11 @@
const float z = raw[Z_AXIS];
raw[Z_AXIS] += z_cxcy;
ubl_buffer_segment_raw(raw, feedrate);
planner.buffer_line(raw, feedrate, active_extruder, segment_xyz_mm
#if ENABLED(SCARA_FEEDRATE_SCALING)
, inv_duration
#endif
);
raw[Z_AXIS] = z;
if (segments == 0) // done with last segment

47
Marlin/src/feature/fwretract.cpp
View File

@ -54,6 +54,7 @@ float FWRetract::retract_length, // M207 S - G10 Retract len
FWRetract::swap_retract_length, // M207 W - G10 Swap Retract length
FWRetract::swap_retract_recover_length, // M208 W - G11 Swap Recover length
FWRetract::swap_retract_recover_feedrate_mm_s, // M208 R - G11 Swap Recover feedrate
FWRetract::current_retract[EXTRUDERS], // Retract value used by planner
FWRetract::current_hop;
void FWRetract::reset() {
@ -73,6 +74,7 @@ void FWRetract::reset() {
#if EXTRUDERS > 1
retracted_swap[i] = false;
#endif
current_retract[i] = 0.0;
}
}
@ -84,9 +86,6 @@ void FWRetract::reset() {
*
* To simplify the logic, doubled retract/recover moves are ignored.
*
* Note: Z lift is done transparently to the planner. Aborting
* a print between G10 and G11 may corrupt the Z position.
*
* Note: Auto-retract will apply the set Z hop in addition to any Z hop
* included in the G-code. Use M207 Z0 to to prevent double hop.
*/
@ -95,9 +94,6 @@ void FWRetract::retract(const bool retracting
, bool swapping /* =false */
#endif
) {
static float current_hop = 0.0; // Total amount lifted, for use in recover
// Prevent two retracts or recovers in a row
if (retracted[active_extruder] == retracting) return;
@ -129,48 +125,50 @@ void FWRetract::retract(const bool retracting
//*/
const float old_feedrate_mm_s = feedrate_mm_s,
renormalize = RECIPROCAL(planner.e_factor[active_extruder]),
base_retract = swapping ? swap_retract_length : retract_length,
old_z = current_position[Z_AXIS],
old_e = current_position[E_AXIS];
unscale_e = RECIPROCAL(planner.e_factor[active_extruder]),
unscale_fr = 100.0 / feedrate_percentage, // Disable feedrate scaling for retract moves
base_retract = swapping ? swap_retract_length : retract_length;
// The current position will be the destination for E and Z moves
set_destination_from_current();
if (retracting) {
// Retract by moving from a faux E position back to the current E position
feedrate_mm_s = retract_feedrate_mm_s;
destination[E_AXIS] -= base_retract * renormalize;
feedrate_mm_s = retract_feedrate_mm_s * unscale_fr;
current_retract[active_extruder] = base_retract * unscale_e;
prepare_move_to_destination(); // set_current_to_destination
planner.synchronize(); // Wait for move to complete
// Is a Z hop set, and has the hop not yet been done?
if (retract_zlift > 0.01 && !current_hop) { // Apply hop only once
current_hop += retract_zlift; // Add to the hop total (again, only once)
destination[Z_AXIS] += retract_zlift; // Raise Z by the zlift (M207 Z) amount
feedrate_mm_s = planner.max_feedrate_mm_s[Z_AXIS]; // Maximum Z feedrate
feedrate_mm_s = planner.max_feedrate_mm_s[Z_AXIS] * unscale_fr; // Maximum Z feedrate
prepare_move_to_destination(); // Raise up, set_current_to_destination
planner.synchronize(); // Wait for move to complete
}
}
else {
// If a hop was done and Z hasn't changed, undo the Z hop
if (current_hop) {
current_position[Z_AXIS] += current_hop; // Restore the actual Z position
SYNC_PLAN_POSITION_KINEMATIC(); // Unspoof the position planner
feedrate_mm_s = planner.max_feedrate_mm_s[Z_AXIS]; // Z feedrate to max
current_hop = 0.0;
feedrate_mm_s = planner.max_feedrate_mm_s[Z_AXIS] * unscale_fr; // Z feedrate to max
prepare_move_to_destination(); // Lower Z, set_current_to_destination
current_hop = 0.0; // Clear the hop amount
planner.synchronize(); // Wait for move to complete
}
destination[E_AXIS] += (base_retract + (swapping ? swap_retract_recover_length : retract_recover_length)) * renormalize;
feedrate_mm_s = swapping ? swap_retract_recover_feedrate_mm_s : retract_recover_feedrate_mm_s;
const float extra_recover = swapping ? swap_retract_recover_length : retract_recover_length;
if (extra_recover != 0.0) {
current_position[E_AXIS] -= extra_recover; // Adjust the current E position by the extra amount to recover
sync_plan_position_e(); // Sync the planner position so the extra amount is recovered
}
current_retract[active_extruder] = 0.0;
feedrate_mm_s = (swapping ? swap_retract_recover_feedrate_mm_s : retract_recover_feedrate_mm_s) * unscale_fr;
prepare_move_to_destination(); // Recover E, set_current_to_destination
planner.synchronize(); // Wait for move to complete
}
feedrate_mm_s = old_feedrate_mm_s; // Restore original feedrate
current_position[Z_AXIS] = old_z; // Restore Z and E positions
current_position[E_AXIS] = old_e;
SYNC_PLAN_POSITION_KINEMATIC(); // As if the move never took place
retracted[active_extruder] = retracting; // Active extruder now retracted / recovered
// If swap retract/recover update the retracted_swap flag too
@ -194,7 +192,6 @@ void FWRetract::retract(const bool retracting
SERIAL_ECHOLNPAIR("current_position[e] ", current_position[E_AXIS]);
SERIAL_ECHOLNPAIR("current_hop ", current_hop);
//*/
}
#endif // FWRETRACT

3
Marlin/src/feature/fwretract.h
View File

@ -46,7 +46,8 @@ public:
swap_retract_length, // M207 W - G10 Swap Retract length
swap_retract_recover_length, // M208 W - G11 Swap Recover length
swap_retract_recover_feedrate_mm_s, // M208 R - G11 Swap Recover feedrate
current_hop;
current_retract[EXTRUDERS], // Retract value used by planner
current_hop; // Hop value used by planner
FWRetract() { reset(); }

2
Marlin/src/feature/pause.cpp
View File

@ -120,7 +120,7 @@ static bool ensure_safe_temperature(const AdvancedPauseMode mode=ADVANCED_PAUSE_
static void do_pause_e_move(const float &length, const float &fr) {
set_destination_from_current();
destination[E_AXIS] += length / planner.e_factor[active_extruder];
planner.buffer_line_kinematic(destination, fr, active_extruder);
planner.buffer_line(destination, fr, active_extruder);
set_current_from_destination();
planner.synchronize();
}

27
Marlin/src/feature/power_loss_recovery.cpp
View File

@ -39,6 +39,10 @@
#include "../sd/cardreader.h"
#include "../core/serial.h"
#if ENABLED(FWRETRACT)
#include "fwretract.h"
#endif
// Recovery data
job_recovery_info_t job_recovery_info;
JobRecoveryPhase job_recovery_phase = JOB_RECOVERY_IDLE;
@ -90,6 +94,15 @@ extern uint8_t commands_in_queue, cmd_queue_index_r;
SERIAL_PROTOCOLPAIR("leveling: ", int(job_recovery_info.leveling));
SERIAL_PROTOCOLLNPAIR(" fade: ", int(job_recovery_info.fade));
#endif
#if ENABLED(FWRETRACT)
SERIAL_PROTOCOLPGM("retract: ");
for (int8_t e = 0; e < EXTRUDERS; e++) {
SERIAL_PROTOCOL(job_recovery_info.retract[e]);
if (e < EXTRUDERS - 1) SERIAL_CHAR(',');
}
SERIAL_EOL();
SERIAL_PROTOCOLLNPAIR("retract_hop: ", job_recovery_info.retract_hop);
#endif
SERIAL_PROTOCOLLNPAIR("cmd_queue_index_r: ", int(job_recovery_info.cmd_queue_index_r));
SERIAL_PROTOCOLLNPAIR("commands_in_queue: ", int(job_recovery_info.commands_in_queue));
if (recovery)
@ -160,6 +173,15 @@ void check_print_job_recovery() {
}
#endif
#if ENABLED(FWRETRACT)
for (uint8_t e = 0; e < EXTRUDERS; e++) {
if (job_recovery_info.retract[e] != 0.0)
fwretract.current_retract[e] = job_recovery_info.retract[e];
fwretract.retracted[e] = true;
}
fwretract.current_hop = job_recovery_info.retract_hop;
#endif
dtostrf(job_recovery_info.current_position[Z_AXIS] + 2, 1, 3, str_1);
dtostrf(job_recovery_info.current_position[E_AXIS]
#if ENABLED(SAVE_EACH_CMD_MODE)
@ -256,6 +278,11 @@ void save_job_recovery_info() {
);
#endif
#if ENABLED(FWRETRACT)
COPY(job_recovery_info.retract, fwretract.current_retract);
job_recovery_info.retract_hop = fwretract.current_hop;
#endif
// Commands in the queue
job_recovery_info.cmd_queue_index_r = cmd_queue_index_r;
job_recovery_info.commands_in_queue = commands_in_queue;

4
Marlin/src/feature/power_loss_recovery.h
View File

@ -60,6 +60,10 @@ typedef struct {
float fade;
#endif
#if ENABLED(FWRETRACT)
float retract[EXTRUDERS], retract_hop;
#endif
// Command queue
uint8_t cmd_queue_index_r, commands_in_queue;
char command_queue[BUFSIZE][MAX_CMD_SIZE];

2
Marlin/src/gcode/bedlevel/abl/G29.cpp
View File

@ -989,7 +989,7 @@ G29_TYPE GcodeSuite::G29() {
KEEPALIVE_STATE(IN_HANDLER);
if (planner.leveling_active)
SYNC_PLAN_POSITION_KINEMATIC();
sync_plan_position();
#if HAS_BED_PROBE && defined(Z_AFTER_PROBING)
move_z_after_probing();

6
Marlin/src/gcode/calibrate/G28.cpp
View File

@ -101,7 +101,7 @@
if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("Z_SAFE_HOMING >>>");
#endif
SYNC_PLAN_POSITION_KINEMATIC();
sync_plan_position();
/**
* Move the Z probe (or just the nozzle) to the safe homing point
@ -182,7 +182,7 @@ void GcodeSuite::G28(const bool always_home_all) {
#if ENABLED(MARLIN_DEV_MODE)
if (parser.seen('S')) {
LOOP_XYZ(a) set_axis_is_at_home((AxisEnum)a);
SYNC_PLAN_POSITION_KINEMATIC();
sync_plan_position();
SERIAL_ECHOLNPGM("Simulated Homing");
report_current_position();
#if ENABLED(DEBUG_LEVELING_FEATURE)
@ -357,7 +357,7 @@ void GcodeSuite::G28(const bool always_home_all) {
} // home_all || homeZ
#endif // Z_HOME_DIR < 0
SYNC_PLAN_POSITION_KINEMATIC();
sync_plan_position();
#endif // !DELTA (G28)

2
Marlin/src/gcode/calibrate/M852.cpp
View File

@ -87,7 +87,7 @@ void GcodeSuite::M852() {
// When skew is changed the current position changes
if (setval) {
set_current_from_steppers_for_axis(ALL_AXES);
SYNC_PLAN_POSITION_KINEMATIC();
sync_plan_position();
report_current_position();
}

11
Marlin/src/gcode/config/M200-M205.cpp
View File

@ -136,14 +136,17 @@ void GcodeSuite::M205() {
const float junc_dev = parser.value_linear_units();
if (WITHIN(junc_dev, 0.01f, 0.3f)) {
planner.junction_deviation_mm = junc_dev;
planner.recalculate_max_e_jerk();
#if ENABLED(LIN_ADVANCE)
planner.recalculate_max_e_jerk();
#endif
}
else {
SERIAL_ERROR_START();
SERIAL_ERRORLNPGM("?J out of range (0.01 to 0.3)");
}
}
#else
#endif
#if HAS_CLASSIC_JERK
if (parser.seen('X')) planner.max_jerk[X_AXIS] = parser.value_linear_units();
if (parser.seen('Y')) planner.max_jerk[Y_AXIS] = parser.value_linear_units();
if (parser.seen('Z')) {
@ -153,6 +156,8 @@ void GcodeSuite::M205() {
SERIAL_ECHOLNPGM("WARNING! Low Z Jerk may lead to unwanted pauses.");
#endif
}
if (parser.seen('E')) planner.max_jerk[E_AXIS] = parser.value_linear_units();
#if DISABLED(JUNCTION_DEVIATION) || DISABLED(LIN_ADVANCE)
if (parser.seen('E')) planner.max_jerk[E_AXIS] = parser.value_linear_units();
#endif
#endif
}

2
Marlin/src/gcode/config/M92.cpp
View File

@ -39,7 +39,7 @@ void GcodeSuite::M92() {
const float value = parser.value_per_axis_unit((AxisEnum)(E_AXIS + TARGET_EXTRUDER));
if (value < 20) {
float factor = planner.axis_steps_per_mm[E_AXIS + TARGET_EXTRUDER] / value; // increase e constants if M92 E14 is given for netfab.
#if DISABLED(JUNCTION_DEVIATION)
#if HAS_CLASSIC_JERK && (DISABLED(JUNCTION_DEVIATION) || DISABLED(LIN_ADVANCE))
planner.max_jerk[E_AXIS] *= factor;
#endif
planner.max_feedrate_mm_s[E_AXIS + TARGET_EXTRUDER] *= factor;

2
Marlin/src/gcode/geometry/G92.cpp
View File

@ -92,7 +92,7 @@ void GcodeSuite::G92() {
COPY(coordinate_system[active_coordinate_system], position_shift);
#endif
if (didXYZ) SYNC_PLAN_POSITION_KINEMATIC();
if (didXYZ) sync_plan_position();
else if (didE) sync_plan_position_e();
report_current_position();

2
Marlin/src/gcode/host/M114.cpp
View File

@ -56,7 +56,7 @@
float leveled[XYZ] = { current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] };
#if PLANNER_LEVELING
#if HAS_LEVELING
SERIAL_PROTOCOLPGM("Leveled:");
planner.apply_leveling(leveled);
report_xyz(leveled);

81
Marlin/src/gcode/motion/G2_G3.cpp
View File

@ -35,10 +35,6 @@
#include "../../module/scara.h"
#endif
#if HAS_FEEDRATE_SCALING && ENABLED(AUTO_BED_LEVELING_BILINEAR)
#include "../../feature/bedlevel/abl/abl.h"
#endif
#if N_ARC_CORRECTION < 1
#undef N_ARC_CORRECTION
#define N_ARC_CORRECTION 1
@ -139,20 +135,12 @@ void plan_arc(
const float fr_mm_s = MMS_SCALED(feedrate_mm_s);
millis_t next_idle_ms = millis() + 200UL;
#if HAS_FEEDRATE_SCALING
// SCARA needs to scale the feed rate from mm/s to degrees/s
const float inv_segment_length = 1.0f / float(MM_PER_ARC_SEGMENT),
inverse_secs = inv_segment_length * fr_mm_s;
float oldA = planner.position_float[A_AXIS],
oldB = planner.position_float[B_AXIS]
#if ENABLED(DELTA_FEEDRATE_SCALING)
, oldC = planner.position_float[C_AXIS]
#endif
;
#if ENABLED(SCARA_FEEDRATE_SCALING)
const float inv_duration = fr_mm_s / MM_PER_ARC_SEGMENT;
#endif
millis_t next_idle_ms = millis() + 200UL;
#if N_ARC_CORRECTION > 1
int8_t arc_recalc_count = N_ARC_CORRECTION;
#endif
@ -196,57 +184,32 @@ void plan_arc(
clamp_to_software_endstops(raw);
#if HAS_FEEDRATE_SCALING
inverse_kinematics(raw);
ADJUST_DELTA(raw);
#if HAS_LEVELING && !PLANNER_LEVELING
planner.apply_leveling(raw);
#endif
#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.
if (!planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], raw[Z_AXIS], raw[E_AXIS], HYPOT(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB) * inverse_secs, active_extruder, MM_PER_ARC_SEGMENT))
break;
oldA = delta[A_AXIS]; oldB = delta[B_AXIS];
#elif ENABLED(DELTA_FEEDRATE_SCALING)
// For DELTA scale the feed rate from Effector mm/s to Carriage mm/s
// i.e., Complete the linear vector in the given time.
if (!planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], SQRT(sq(delta[A_AXIS] - oldA) + sq(delta[B_AXIS] - oldB) + sq(delta[C_AXIS] - oldC)) * inverse_secs, active_extruder, MM_PER_ARC_SEGMENT))
break;
oldA = delta[A_AXIS]; oldB = delta[B_AXIS]; oldC = delta[C_AXIS];
#elif HAS_UBL_AND_CURVES
float pos[XYZ] = { raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS] };
planner.apply_leveling(pos);
if (!planner.buffer_segment(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], raw[E_AXIS], fr_mm_s, active_extruder, MM_PER_ARC_SEGMENT))
break;
#else
if (!planner.buffer_line_kinematic(raw, fr_mm_s, active_extruder))
break;
#endif
if (!planner.buffer_line(raw, fr_mm_s, active_extruder, MM_PER_ARC_SEGMENT
#if ENABLED(SCARA_FEEDRATE_SCALING)
, inv_duration
#endif
))
break;
}
// Ensure last segment arrives at target location.
#if HAS_FEEDRATE_SCALING
inverse_kinematics(cart);
ADJUST_DELTA(cart);
COPY(raw, cart);
#if HAS_LEVELING && !PLANNER_LEVELING
planner.apply_leveling(raw);
#endif
#if ENABLED(SCARA_FEEDRATE_SCALING)
const float diff2 = HYPOT2(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB);
if (diff2)
planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], cart[Z_AXIS], cart[E_AXIS], SQRT(diff2) * inverse_secs, active_extruder, MM_PER_ARC_SEGMENT);
#elif ENABLED(DELTA_FEEDRATE_SCALING)
const float diff2 = sq(delta[A_AXIS] - oldA) + sq(delta[B_AXIS] - oldB) + sq(delta[C_AXIS] - oldC);
if (diff2)
planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], cart[E_AXIS], SQRT(diff2) * inverse_secs, active_extruder, MM_PER_ARC_SEGMENT);
#elif HAS_UBL_AND_CURVES
float pos[XYZ] = { cart[X_AXIS], cart[Y_AXIS], cart[Z_AXIS] };
planner.apply_leveling(pos);
planner.buffer_segment(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], cart[E_AXIS], fr_mm_s, active_extruder, MM_PER_ARC_SEGMENT);
#else
planner.buffer_line_kinematic(cart, fr_mm_s, active_extruder);
#endif
planner.buffer_line(raw, fr_mm_s, active_extruder, MM_PER_ARC_SEGMENT
#if ENABLED(SCARA_FEEDRATE_SCALING)
, inv_duration
#endif
);
COPY(current_position, cart);
COPY(current_position, raw);
} // plan_arc
/**

4
Marlin/src/gcode/probe/G38.cpp
View File

@ -56,7 +56,7 @@ static bool G38_run_probe() {
endstops.hit_on_purpose();
set_current_from_steppers_for_axis(ALL_AXES);
SYNC_PLAN_POSITION_KINEMATIC();
sync_plan_position();
if (G38_endstop_hit) {
@ -82,7 +82,7 @@ static bool G38_run_probe() {
G38_move = false;
set_current_from_steppers_for_axis(ALL_AXES);
SYNC_PLAN_POSITION_KINEMATIC();
sync_plan_position();
#endif
}

7
Marlin/src/inc/Conditionals_post.h
View File

@ -49,6 +49,8 @@
#define IS_KINEMATIC (ENABLED(DELTA) || IS_SCARA)
#define IS_CARTESIAN !IS_KINEMATIC
#define HAS_CLASSIC_JERK (IS_KINEMATIC || DISABLED(JUNCTION_DEVIATION))
/**
* Axis lengths and center
*/
@ -1173,10 +1175,9 @@
#define HAS_LEVELING (HAS_ABL || ENABLED(MESH_BED_LEVELING))
#define HAS_AUTOLEVEL (HAS_ABL && DISABLED(PROBE_MANUALLY))
#define HAS_MESH (ENABLED(AUTO_BED_LEVELING_BILINEAR) || ENABLED(AUTO_BED_LEVELING_UBL) || ENABLED(MESH_BED_LEVELING))
#define PLANNER_LEVELING (OLDSCHOOL_ABL || ENABLED(MESH_BED_LEVELING) || UBL_SEGMENTED || ENABLED(SKEW_CORRECTION))
#define PLANNER_LEVELING (HAS_LEVELING && DISABLED(AUTO_BED_LEVELING_UBL))
#define HAS_PROBING_PROCEDURE (HAS_ABL || ENABLED(Z_MIN_PROBE_REPEATABILITY_TEST))
#define HAS_UBL_AND_CURVES (ENABLED(AUTO_BED_LEVELING_UBL) && !PLANNER_LEVELING && (ENABLED(ARC_SUPPORT) || ENABLED(BEZIER_CURVE_SUPPORT)))
#define HAS_FEEDRATE_SCALING (ENABLED(SCARA_FEEDRATE_SCALING) || ENABLED(DELTA_FEEDRATE_SCALING))
#define HAS_POSITION_MODIFIERS (ENABLED(FWRETRACT) || HAS_LEVELING || ENABLED(SKEW_CORRECTION))
#if ENABLED(AUTO_BED_LEVELING_UBL)
#undef LCD_BED_LEVELING

21
Marlin/src/lcd/ultralcd.cpp
View File

@ -841,7 +841,7 @@ void lcd_quick_feedback(const bool clear_buttons) {
}
inline void line_to_current_z() {
planner.buffer_line_kinematic(current_position, MMM_TO_MMS(manual_feedrate_mm_m[Z_AXIS]), active_extruder);
planner.buffer_line(current_position, MMM_TO_MMS(manual_feedrate_mm_m[Z_AXIS]), active_extruder);
}
inline void line_to_z(const float &z) {
@ -1892,7 +1892,7 @@ void lcd_quick_feedback(const bool clear_buttons) {
break;
#endif
}
planner.buffer_line_kinematic(current_position, MMM_TO_MMS(manual_feedrate_mm_m[X_AXIS]), active_extruder);
planner.buffer_line(current_position, MMM_TO_MMS(manual_feedrate_mm_m[X_AXIS]), active_extruder);
line_to_z(0.0);
if (++bed_corner > 3
#if ENABLED(LEVEL_CENTER_TOO)
@ -2432,7 +2432,7 @@ void lcd_quick_feedback(const bool clear_buttons) {
void ubl_map_move_to_xy() {
current_position[X_AXIS] = pgm_read_float(&ubl._mesh_index_to_xpos[x_plot]);
current_position[Y_AXIS] = pgm_read_float(&ubl._mesh_index_to_ypos[y_plot]);
planner.buffer_line_kinematic(current_position, MMM_TO_MMS(XY_PROBE_SPEED), active_extruder);
planner.buffer_line(current_position, MMM_TO_MMS(XY_PROBE_SPEED), active_extruder);
}
/**
@ -2911,7 +2911,7 @@ void lcd_quick_feedback(const bool clear_buttons) {
#else
planner.buffer_line_kinematic(current_position, MMM_TO_MMS(manual_feedrate_mm_m[manual_move_axis]), manual_move_axis == E_AXIS ? manual_move_e_index : active_extruder);
planner.buffer_line(current_position, MMM_TO_MMS(manual_feedrate_mm_m[manual_move_axis]), manual_move_axis == E_AXIS ? manual_move_e_index : active_extruder);
manual_move_axis = (int8_t)NO_AXIS;
#endif
@ -3746,8 +3746,13 @@ void lcd_quick_feedback(const bool clear_buttons) {
MENU_BACK(MSG_ADVANCED_SETTINGS);
#if ENABLED(JUNCTION_DEVIATION)
MENU_ITEM_EDIT_CALLBACK(float43, MSG_JUNCTION_DEVIATION, &planner.junction_deviation_mm, 0.01f, 0.3f, planner.recalculate_max_e_jerk);
#else
#if ENABLED(LIN_ADVANCE)
MENU_ITEM_EDIT_CALLBACK(float43, MSG_JUNCTION_DEVIATION, &planner.junction_deviation_mm, 0.01f, 0.3f, planner.recalculate_max_e_jerk);
#else
MENU_ITEM_EDIT(float43, MSG_JUNCTION_DEVIATION, &planner.junction_deviation_mm, 0.01f, 0.3f);
#endif
#endif
#if HAS_CLASSIC_JERK
MENU_MULTIPLIER_ITEM_EDIT(float3, MSG_VA_JERK, &planner.max_jerk[A_AXIS], 1, 990);
MENU_MULTIPLIER_ITEM_EDIT(float3, MSG_VB_JERK, &planner.max_jerk[B_AXIS], 1, 990);
#if ENABLED(DELTA)
@ -3755,7 +3760,9 @@ void lcd_quick_feedback(const bool clear_buttons) {
#else
MENU_MULTIPLIER_ITEM_EDIT(float52sign, MSG_VC_JERK, &planner.max_jerk[C_AXIS], 0.1f, 990);
#endif
MENU_MULTIPLIER_ITEM_EDIT(float3, MSG_VE_JERK, &planner.max_jerk[E_AXIS], 1, 990);
#if DISABLED(JUNCTION_DEVIATION) || DISABLED(LIN_ADVANCE)
MENU_MULTIPLIER_ITEM_EDIT(float3, MSG_VE_JERK, &planner.max_jerk[E_AXIS], 1, 990);
#endif
#endif
END_MENU();

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

@ -428,12 +428,20 @@ void MarlinSettings::postprocess() {
EEPROM_WRITE(planner.min_feedrate_mm_s);
EEPROM_WRITE(planner.min_travel_feedrate_mm_s);
#if ENABLED(JUNCTION_DEVIATION)
#if HAS_CLASSIC_JERK
EEPROM_WRITE(planner.max_jerk);
#if ENABLED(JUNCTION_DEVIATION) && ENABLED(LIN_ADVANCE)
dummy = float(DEFAULT_EJERK);
EEPROM_WRITE(dummy);
#endif
#else
const float planner_max_jerk[] = { float(DEFAULT_XJERK), float(DEFAULT_YJERK), float(DEFAULT_ZJERK), float(DEFAULT_EJERK) };
EEPROM_WRITE(planner_max_jerk);
#endif
#if ENABLED(JUNCTION_DEVIATION)
EEPROM_WRITE(planner.junction_deviation_mm);
#else
EEPROM_WRITE(planner.max_jerk);
dummy = 0.02f;
EEPROM_WRITE(dummy);
#endif
@ -1062,11 +1070,18 @@ void MarlinSettings::postprocess() {
EEPROM_READ(planner.min_feedrate_mm_s);
EEPROM_READ(planner.min_travel_feedrate_mm_s);
#if ENABLED(JUNCTION_DEVIATION)
#if HAS_CLASSIC_JERK
EEPROM_READ(planner.max_jerk);
#if ENABLED(JUNCTION_DEVIATION) && ENABLED(LIN_ADVANCE)
EEPROM_READ(dummy);
#endif
#else
for (uint8_t q = 4; q--;) EEPROM_READ(dummy);
#endif
#if ENABLED(JUNCTION_DEVIATION)
EEPROM_READ(planner.junction_deviation_mm);
#else
EEPROM_READ(planner.max_jerk);
EEPROM_READ(dummy);
#endif
@ -1808,11 +1823,15 @@ void MarlinSettings::reset(PORTARG_SOLO) {
#if ENABLED(JUNCTION_DEVIATION)
planner.junction_deviation_mm = float(JUNCTION_DEVIATION_MM);
#else
#endif
#if HAS_CLASSIC_JERK
planner.max_jerk[X_AXIS] = DEFAULT_XJERK;
planner.max_jerk[Y_AXIS] = DEFAULT_YJERK;
planner.max_jerk[Z_AXIS] = DEFAULT_ZJERK;
planner.max_jerk[E_AXIS] = DEFAULT_EJERK;
#if DISABLED(JUNCTION_DEVIATION) || DISABLED(LIN_ADVANCE)
planner.max_jerk[E_AXIS] = DEFAULT_EJERK;
#endif
#endif
#if HAS_HOME_OFFSET
@ -2243,11 +2262,12 @@ void MarlinSettings::reset(PORTARG_SOLO) {
SERIAL_ECHOPGM_P(port, "Advanced: B<min_segment_time_us> S<min_feedrate> T<min_travel_feedrate>");
#if ENABLED(JUNCTION_DEVIATION)
SERIAL_ECHOPGM_P(port, " J<junc_dev>");
#else
SERIAL_ECHOPGM_P(port, " X<max_x_jerk> Y<max_y_jerk> Z<max_z_jerk>");
#endif
#if DISABLED(JUNCTION_DEVIATION) || ENABLED(LIN_ADVANCE)
SERIAL_ECHOPGM_P(port, " E<max_e_jerk>");
#if HAS_CLASSIC_JERK
SERIAL_ECHOPGM_P(port, " X<max_x_jerk> Y<max_y_jerk> Z<max_z_jerk>");
#if DISABLED(JUNCTION_DEVIATION) || DISABLED(LIN_ADVANCE)
SERIAL_ECHOPGM_P(port, " E<max_e_jerk>");
#endif
#endif
SERIAL_EOL_P(port);
}
@ -2258,11 +2278,14 @@ void MarlinSettings::reset(PORTARG_SOLO) {
#if ENABLED(JUNCTION_DEVIATION)
SERIAL_ECHOPAIR_P(port, " J", LINEAR_UNIT(planner.junction_deviation_mm));
#else
#endif
#if HAS_CLASSIC_JERK
SERIAL_ECHOPAIR_P(port, " X", LINEAR_UNIT(planner.max_jerk[X_AXIS]));
SERIAL_ECHOPAIR_P(port, " Y", LINEAR_UNIT(planner.max_jerk[Y_AXIS]));
SERIAL_ECHOPAIR_P(port, " Z", LINEAR_UNIT(planner.max_jerk[Z_AXIS]));
SERIAL_ECHOPAIR_P(port, " E", LINEAR_UNIT(planner.max_jerk[E_AXIS]));
#if DISABLED(JUNCTION_DEVIATION) || DISABLED(LIN_ADVANCE)
SERIAL_ECHOPAIR_P(port, " E", LINEAR_UNIT(planner.max_jerk[E_AXIS]));
#endif
#endif
SERIAL_EOL_P(port);

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

@ -101,7 +101,7 @@ void recalc_delta_settings() {
SERIAL_ECHOLNPAIR(" C:", delta[C_AXIS]); \
}while(0)
void inverse_kinematics(const float raw[XYZ]) {
void inverse_kinematics(const float (&raw)[XYZ]) {
#if HAS_HOTEND_OFFSET
// Delta hotend offsets must be applied in Cartesian space with no "spoofing"
const float pos[XYZ] = {
@ -224,6 +224,7 @@ void home_delta() {
#endif
// Init the current position of all carriages to 0,0,0
ZERO(current_position);
ZERO(destination);
sync_plan_position();
// Disable stealthChop if used. Enable diag1 pin on driver.
@ -232,9 +233,8 @@ void home_delta() {
#endif
// Move all carriages together linearly until an endstop is hit.
current_position[X_AXIS] = current_position[Y_AXIS] = current_position[Z_AXIS] = (delta_height + 10);
feedrate_mm_s = homing_feedrate(X_AXIS);
line_to_current_position();
destination[Z_AXIS] = (delta_height + 10);
buffer_line_to_destination(homing_feedrate(X_AXIS));
planner.synchronize();
// Re-enable stealthChop if used. Disable diag1 pin on driver.
@ -256,7 +256,7 @@ void home_delta() {
// give the impression that they are the same.
LOOP_XYZ(i) set_axis_is_at_home((AxisEnum)i);
SYNC_PLAN_POSITION_KINEMATIC();
sync_plan_position();
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("<<< home_delta", current_position);

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

@ -24,8 +24,7 @@
* delta.h - Delta-specific functions
*/
#ifndef __DELTA_H__
#define __DELTA_H__
#pragma once
extern float delta_height,
delta_endstop_adj[ABC],
@ -78,7 +77,11 @@ void recalc_delta_settings();
delta[C_AXIS] = DELTA_Z(V, C_AXIS); \
}while(0)
void inverse_kinematics(const float raw[XYZ]);
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);
}
/**
* Calculate the highest Z position where the
@ -118,5 +121,3 @@ FORCE_INLINE void forward_kinematics_DELTA(const float (&point)[ABC]) {
}
void home_delta();
#endif // __DELTA_H__

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

@ -75,7 +75,7 @@ bool relative_mode; // = false;
* Cartesian Current Position
* Used to track the native machine position as moves are queued.
* Used by 'buffer_line_to_current_position' to do a move after changing it.
* Used by 'SYNC_PLAN_POSITION_KINEMATIC' to update 'planner.position'.
* Used by 'sync_plan_position' to update 'planner.position'.
*/
float current_position[XYZE] = { 0 };
@ -218,15 +218,22 @@ void get_cartesian_from_steppers() {
* may have been applied.
*
* To prevent small shifts in axis position always call
* SYNC_PLAN_POSITION_KINEMATIC after updating axes with this.
* sync_plan_position after updating axes with this.
*
* To keep hosts in sync, always call report_current_position
* after updating the current_position.
*/
void set_current_from_steppers_for_axis(const AxisEnum axis) {
get_cartesian_from_steppers();
#if PLANNER_LEVELING
planner.unapply_leveling(cartes);
#if HAS_POSITION_MODIFIERS
float pos[XYZE] = { cartes[X_AXIS], cartes[Y_AXIS], cartes[Z_AXIS], current_position[E_AXIS] };
planner.unapply_modifiers(pos
#if HAS_LEVELING
, true
#endif
);
const float (&cartes)[XYZE] = pos;
#endif
if (axis == ALL_AXES)
COPY(current_position, cartes);
@ -252,13 +259,6 @@ void buffer_line_to_destination(const float fr_mm_s) {
#if IS_KINEMATIC
void sync_plan_position_kinematic() {
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("sync_plan_position_kinematic", current_position);
#endif
planner.set_position_mm_kinematic(current_position);
}
/**
* Calculate delta, start a line, and set current_position to destination
*/
@ -277,7 +277,7 @@ void buffer_line_to_destination(const float fr_mm_s) {
&& current_position[E_AXIS] == destination[E_AXIS]
) return;
planner.buffer_line_kinematic(destination, MMS_SCALED(fr_mm_s ? fr_mm_s : feedrate_mm_s), active_extruder);
planner.buffer_line(destination, MMS_SCALED(fr_mm_s ? fr_mm_s : feedrate_mm_s), active_extruder);
#endif
set_current_from_destination();
@ -538,7 +538,7 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
// If the move is only in Z/E don't split up the move
if (!xdiff && !ydiff) {
planner.buffer_line_kinematic(rtarget, _feedrate_mm_s, active_extruder);
planner.buffer_line(rtarget, _feedrate_mm_s, active_extruder);
return false; // caller will update current_position
}
@ -580,53 +580,22 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
ydiff * inv_segments,
zdiff * inv_segments,
ediff * inv_segments
};
#if !HAS_FEEDRATE_SCALING
const float cartesian_segment_mm = cartesian_mm * inv_segments;
},
cartesian_segment_mm = cartesian_mm * inv_segments;
#if ENABLED(SCARA_FEEDRATE_SCALING)
const float inv_duration = _feedrate_mm_s / cartesian_segment_mm;
#endif
/*
SERIAL_ECHOPAIR("mm=", cartesian_mm);
SERIAL_ECHOPAIR(" seconds=", seconds);
SERIAL_ECHOPAIR(" segments=", segments);
#if !HAS_FEEDRATE_SCALING
SERIAL_ECHOPAIR(" segment_mm=", cartesian_segment_mm);
#endif
SERIAL_ECHOPAIR(" segment_mm=", cartesian_segment_mm);
SERIAL_EOL();
//*/
#if HAS_FEEDRATE_SCALING
// SCARA needs to scale the feed rate from mm/s to degrees/s
// i.e., Complete the angular vector in the given time.
const float segment_length = cartesian_mm * inv_segments,
inv_segment_length = 1.0f / segment_length, // 1/mm/segs
inverse_secs = inv_segment_length * _feedrate_mm_s;
float oldA = planner.position_float[A_AXIS],
oldB = planner.position_float[B_AXIS]
#if ENABLED(DELTA_FEEDRATE_SCALING)
, oldC = planner.position_float[C_AXIS]
#endif
;
/*
SERIAL_ECHOPGM("Scaled kinematic move: ");
SERIAL_ECHOPAIR(" segment_length (inv)=", segment_length);
SERIAL_ECHOPAIR(" (", inv_segment_length);
SERIAL_ECHOPAIR(") _feedrate_mm_s=", _feedrate_mm_s);
SERIAL_ECHOPAIR(" inverse_secs=", inverse_secs);
SERIAL_ECHOPAIR(" oldA=", oldA);
SERIAL_ECHOPAIR(" oldB=", oldB);
#if ENABLED(DELTA_FEEDRATE_SCALING)
SERIAL_ECHOPAIR(" oldC=", oldC);
#endif
SERIAL_EOL();
safe_delay(5);
//*/
#endif
// Get the current position as starting point
// Get the current position as starting point
float raw[XYZE];
COPY(raw, current_position);
@ -642,78 +611,20 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
LOOP_XYZE(i) raw[i] += segment_distance[i];
#if ENABLED(DELTA) && HOTENDS < 2
DELTA_IK(raw); // Delta can inline its kinematics
#else
inverse_kinematics(raw);
#endif
ADJUST_DELTA(raw); // Adjust Z if bed leveling is enabled
#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.
if (!planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], raw[Z_AXIS], raw[E_AXIS], HYPOT(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB) * inverse_secs, active_extruder, segment_length))
break;
/*
SERIAL_ECHO(segments);
SERIAL_ECHOPAIR(": X=", raw[X_AXIS]); SERIAL_ECHOPAIR(" Y=", raw[Y_AXIS]);
SERIAL_ECHOPAIR(" A=", delta[A_AXIS]); SERIAL_ECHOPAIR(" B=", delta[B_AXIS]);
SERIAL_ECHOLNPAIR(" F", HYPOT(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB) * inverse_secs * 60);
safe_delay(5);
//*/
oldA = delta[A_AXIS]; oldB = delta[B_AXIS];
#elif ENABLED(DELTA_FEEDRATE_SCALING)
// For DELTA scale the feed rate from Effector mm/s to Carriage mm/s
// i.e., Complete the linear vector in the given time.
if (!planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], SQRT(sq(delta[A_AXIS] - oldA) + sq(delta[B_AXIS] - oldB) + sq(delta[C_AXIS] - oldC)) * inverse_secs, active_extruder, segment_length))
break;
/*
SERIAL_ECHO(segments);
SERIAL_ECHOPAIR(": X=", raw[X_AXIS]); SERIAL_ECHOPAIR(" Y=", raw[Y_AXIS]);
SERIAL_ECHOPAIR(" A=", delta[A_AXIS]); SERIAL_ECHOPAIR(" B=", delta[B_AXIS]); SERIAL_ECHOPAIR(" C=", delta[C_AXIS]);
SERIAL_ECHOLNPAIR(" F", SQRT(sq(delta[A_AXIS] - oldA) + sq(delta[B_AXIS] - oldB) + sq(delta[C_AXIS] - oldC)) * inverse_secs * 60);
safe_delay(5);
//*/
oldA = delta[A_AXIS]; oldB = delta[B_AXIS]; oldC = delta[C_AXIS];
#else
if (!planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], _feedrate_mm_s, active_extruder, cartesian_segment_mm))
break;
#endif
if (!planner.buffer_line(raw, _feedrate_mm_s, active_extruder, cartesian_segment_mm
#if ENABLED(SCARA_FEEDRATE_SCALING)
, inv_duration
#endif
))
break;
}
// Ensure last segment arrives at target location.
#if HAS_FEEDRATE_SCALING
inverse_kinematics(rtarget);
ADJUST_DELTA(rtarget);
#endif
#if ENABLED(SCARA_FEEDRATE_SCALING)
const float diff2 = HYPOT2(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB);
if (diff2) {
planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], rtarget[Z_AXIS], rtarget[E_AXIS], SQRT(diff2) * inverse_secs, active_extruder, segment_length);
/*
SERIAL_ECHOPAIR("final: A=", delta[A_AXIS]); SERIAL_ECHOPAIR(" B=", delta[B_AXIS]);
SERIAL_ECHOPAIR(" adiff=", delta[A_AXIS] - oldA); SERIAL_ECHOPAIR(" bdiff=", delta[B_AXIS] - oldB);
SERIAL_ECHOLNPAIR(" F", SQRT(diff2) * inverse_secs * 60);
SERIAL_EOL();
safe_delay(5);
//*/
}
#elif ENABLED(DELTA_FEEDRATE_SCALING)
const float diff2 = sq(delta[A_AXIS] - oldA) + sq(delta[B_AXIS] - oldB) + sq(delta[C_AXIS] - oldC);
if (diff2) {
planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], rtarget[E_AXIS], SQRT(diff2) * inverse_secs, active_extruder, segment_length);
/*
SERIAL_ECHOPAIR("final: A=", delta[A_AXIS]); SERIAL_ECHOPAIR(" B=", delta[B_AXIS]); SERIAL_ECHOPAIR(" C=", delta[C_AXIS]);
SERIAL_ECHOPAIR(" adiff=", delta[A_AXIS] - oldA); SERIAL_ECHOPAIR(" bdiff=", delta[B_AXIS] - oldB); SERIAL_ECHOPAIR(" cdiff=", delta[C_AXIS] - oldC);
SERIAL_ECHOLNPAIR(" F", SQRT(diff2) * inverse_secs * 60);
SERIAL_EOL();
safe_delay(5);
//*/
}
#else
planner.buffer_line_kinematic(rtarget, _feedrate_mm_s, active_extruder, cartesian_segment_mm);
#endif
planner.buffer_line(rtarget, _feedrate_mm_s, active_extruder, cartesian_segment_mm
#if ENABLED(SCARA_FEEDRATE_SCALING)
, inv_duration
#endif
);
return false; // caller will update current_position
}
@ -736,7 +647,7 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
// If the move is only in Z/E don't split up the move
if (!xdiff && !ydiff) {
planner.buffer_line_kinematic(destination, fr_mm_s, active_extruder);
planner.buffer_line(destination, fr_mm_s, active_extruder);
return;
}
@ -766,6 +677,10 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
ediff * inv_segments
};
#if ENABLED(SCARA_FEEDRATE_SCALING)
const float inv_duration = _feedrate_mm_s / cartesian_segment_mm;
#endif
// SERIAL_ECHOPAIR("mm=", cartesian_mm);
// SERIAL_ECHOLNPAIR(" segments=", segments);
// SERIAL_ECHOLNPAIR(" segment_mm=", cartesian_segment_mm);
@ -783,13 +698,21 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
idle();
}
LOOP_XYZE(i) raw[i] += segment_distance[i];
if (!planner.buffer_line_kinematic(raw, fr_mm_s, active_extruder, cartesian_segment_mm))
if (!planner.buffer_line(raw, fr_mm_s, active_extruder, cartesian_segment_mm
#if ENABLED(SCARA_FEEDRATE_SCALING)
, inv_duration
#endif
))
break;
}
// Since segment_distance is only approximate,
// the final move must be to the exact destination.
planner.buffer_line_kinematic(destination, fr_mm_s, active_extruder, cartesian_segment_mm);
planner.buffer_line(destination, fr_mm_s, active_extruder, cartesian_segment_mm
#if ENABLED(SCARA_FEEDRATE_SCALING)
, inv_duration
#endif
);
}
#endif // SEGMENT_LEVELED_MOVES
@ -922,7 +845,7 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
planner.max_feedrate_mm_s[X_AXIS], 1)
) break;
planner.synchronize();
SYNC_PLAN_POSITION_KINEMATIC();
sync_plan_position();
extruder_duplication_enabled = true;
active_extruder_parked = false;
#if ENABLED(DEBUG_LEVELING_FEATURE)
@ -1092,7 +1015,7 @@ inline float get_homing_bump_feedrate(const AxisEnum axis) {
/**
* Home an individual linear axis
*/
static void do_homing_move(const AxisEnum axis, const float distance, const float fr_mm_s=0.0) {
void do_homing_move(const AxisEnum axis, const float distance, const float fr_mm_s=0.0) {
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) {
@ -1139,18 +1062,29 @@ static void do_homing_move(const AxisEnum axis, const float distance, const floa
#endif
}
// Tell the planner the axis is at 0
current_position[axis] = 0;
#if IS_SCARA
SYNC_PLAN_POSITION_KINEMATIC();
current_position[axis] = distance;
inverse_kinematics(current_position);
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], current_position[E_AXIS], fr_mm_s ? fr_mm_s : homing_feedrate(axis), active_extruder);
#else
// Tell the planner the axis is at 0
current_position[axis] = 0;
sync_plan_position();
current_position[axis] = distance; // Set delta/cartesian axes directly
planner.buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], fr_mm_s ? fr_mm_s : homing_feedrate(axis), active_extruder);
current_position[axis] = distance;
planner.buffer_line(current_position, fr_mm_s ? fr_mm_s : homing_feedrate(axis), active_extruder);
#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) };
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};
#endif
// Set delta/cartesian axes directly
planner.buffer_segment(target
#if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION)
, delta_mm_cart
#endif
, fr_mm_s ? fr_mm_s : homing_feedrate(axis), active_extruder
);
#endif
planner.synchronize();
@ -1349,7 +1283,14 @@ void homeaxis(const AxisEnum axis) {
if (axis == Z_AXIS && set_bltouch_deployed(true)) return;
#endif
do_homing_move(axis, 1.5f * max_length(axis) * axis_home_dir);
do_homing_move(axis, 1.5f * max_length(
#if ENABLED(DELTA)
Z_AXIS
#else
axis
#endif
) * axis_home_dir
);
#if HOMING_Z_WITH_PROBE && ENABLED(BLTOUCH)
// BLTOUCH needs to be stowed after trigger to rearm itself
@ -1481,7 +1422,7 @@ void homeaxis(const AxisEnum axis) {
#if IS_SCARA
set_axis_is_at_home(axis);
SYNC_PLAN_POSITION_KINEMATIC();
sync_plan_position();
#elif ENABLED(DELTA)

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

@ -129,13 +129,6 @@ void set_current_from_steppers_for_axis(const AxisEnum axis);
void sync_plan_position();
void sync_plan_position_e();
#if IS_KINEMATIC
void sync_plan_position_kinematic();
#define SYNC_PLAN_POSITION_KINEMATIC() sync_plan_position_kinematic()
#else
#define SYNC_PLAN_POSITION_KINEMATIC() sync_plan_position()
#endif
/**
* Move the planner to the current position from wherever it last moved
* (or from wherever it has been told it is located).
@ -354,20 +347,4 @@ void homeaxis(const AxisEnum axis);
void set_home_offset(const AxisEnum axis, const float v);
#endif
#if ENABLED(AUTO_BED_LEVELING_BILINEAR)
#if ENABLED(DELTA)
#define ADJUST_DELTA(V) \
if (planner.leveling_active) { \
const float zadj = bilinear_z_offset(V); \
delta[A_AXIS] += zadj; \
delta[B_AXIS] += zadj; \
delta[C_AXIS] += zadj; \
}
#else
#define ADJUST_DELTA(V) if (planner.leveling_active) { delta[Z_AXIS] += bilinear_z_offset(V); }
#endif
#else
#define ADJUST_DELTA(V) NOOP
#endif
#endif // MOTION_H

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

@ -133,8 +133,13 @@ float Planner::max_feedrate_mm_s[XYZE_N], // (mm/s) M203 XYZE - Max speeds
float Planner::max_e_jerk;
#endif
#endif
#else
float Planner::max_jerk[XYZE]; // (mm/s^2) M205 XYZE - The largest speed change requiring no acceleration.
#endif
#if HAS_CLASSIC_JERK
#if ENABLED(JUNCTION_DEVIATION) && ENABLED(LIN_ADVANCE)
float Planner::max_jerk[XYZ]; // (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.
#endif
#endif
#if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
@ -220,6 +225,10 @@ float Planner::previous_speed[NUM_AXIS],
float Planner::position_float[XYZE]; // Needed for accurate maths. Steps cannot be used!
#endif
#if IS_KINEMATIC
float Planner::position_cart[XYZE];
#endif
#if ENABLED(ULTRA_LCD)
volatile uint32_t Planner::block_buffer_runtime_us = 0;
#endif
@ -235,6 +244,9 @@ void Planner::init() {
#if HAS_POSITION_FLOAT
ZERO(position_float);
#endif
#if IS_KINEMATIC
ZERO(position_cart);
#endif
ZERO(previous_speed);
previous_nominal_speed_sqr = 0;
#if ABL_PLANAR
@ -1354,17 +1366,12 @@ void Planner::check_axes_activity() {
}
#endif
#if PLANNER_LEVELING || HAS_UBL_AND_CURVES
#if HAS_LEVELING
/**
* rx, ry, rz - Cartesian positions in mm
* Leveled XYZ on completion
*/
void Planner::apply_leveling(float &rx, float &ry, float &rz) {
#if ENABLED(SKEW_CORRECTION)
skew(rx, ry, rz);
#endif
if (!leveling_active) return;
#if ABL_PLANAR
@ -1406,10 +1413,6 @@ void Planner::check_axes_activity() {
#endif
}
#endif
#if PLANNER_LEVELING
void Planner::unapply_leveling(float raw[XYZ]) {
if (leveling_active) {
@ -1456,7 +1459,23 @@ void Planner::check_axes_activity() {
#endif
}
#endif // PLANNER_LEVELING
#endif // HAS_LEVELING
#if ENABLED(FWRETRACT)
/**
* rz, e - Cartesian positions in mm
*/
void Planner::apply_retract(float &rz, float &e) {
rz += fwretract.current_hop;
e -= fwretract.current_retract[active_extruder];
}
void Planner::unapply_retract(float &rz, float &e) {
rz -= fwretract.current_hop;
e += fwretract.current_retract[active_extruder];
}
#endif
void Planner::quick_stop() {
@ -1554,6 +1573,7 @@ void Planner::synchronize() {
* Add a new linear movement to the planner queue (in terms of steps).
*
* target - target position in steps units
* target_float - target position in direct (mm, degrees) units. optional
* fr_mm_s - (target) speed of the move
* extruder - target extruder
* millimeters - the length of the movement, if known
@ -1562,7 +1582,10 @@ void Planner::synchronize() {
*/
bool Planner::_buffer_steps(const int32_t (&target)[XYZE]
#if HAS_POSITION_FLOAT
, const float (&target_float)[XYZE]
, const float (&target_float)[ABCE]
#endif
#if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION)
, const float (&delta_mm_cart)[XYZE]
#endif
, float fr_mm_s, const uint8_t extruder, const float &millimeters
) {
@ -1579,6 +1602,9 @@ bool Planner::_buffer_steps(const int32_t (&target)[XYZE]
#if HAS_POSITION_FLOAT
, target_float
#endif
#if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION)
, delta_mm_cart
#endif
, fr_mm_s, extruder, millimeters
)) {
// Movement was not queued, probably because it was too short.
@ -1618,9 +1644,12 @@ 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)[XYZE]
const int32_t (&target)[ABCE]
#if HAS_POSITION_FLOAT
, const float (&target_float)[XYZE]
, const float (&target_float)[ABCE]
#endif
#if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION)
, const float (&delta_mm_cart)[XYZE]
#endif
, float fr_mm_s, const uint8_t extruder, const float &millimeters/*=0.0*/
) {
@ -2243,12 +2272,21 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
// Unit vector of previous path line segment
static float previous_unit_vec[XYZE];
float unit_vec[] = {
delta_mm[A_AXIS] * inverse_millimeters,
delta_mm[B_AXIS] * inverse_millimeters,
delta_mm[C_AXIS] * inverse_millimeters,
delta_mm[E_AXIS] * inverse_millimeters
};
#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
// Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
if (moves_queued && !UNEAR_ZERO(previous_nominal_speed_sqr)) {
@ -2302,7 +2340,9 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
COPY(previous_unit_vec, unit_vec);
#else // Classic Jerk Limiting
#endif
#if HAS_CLASSIC_JERK
/**
* Adapted from Průša MKS firmware
@ -2317,7 +2357,12 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
float safe_speed = nominal_speed;
uint8_t limited = 0;
LOOP_XYZE(i) {
#if ENABLED(JUNCTION_DEVIATION) && ENABLED(LIN_ADVANCE)
LOOP_XYZ(i)
#else
LOOP_XYZE(i)
#endif
{
const float jerk = ABS(current_speed[i]), // cs : Starting from zero, change in speed for this axis
maxj = max_jerk[i]; // mj : The max jerk setting for this axis
if (jerk > maxj) { // cs > mj : New current speed too fast?
@ -2349,7 +2394,12 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
// Now limit the jerk in all axes.
const float smaller_speed_factor = vmax_junction / previous_nominal_speed;
LOOP_XYZE(axis) {
#if ENABLED(JUNCTION_DEVIATION) && ENABLED(LIN_ADVANCE)
LOOP_XYZ(axis)
#else
LOOP_XYZE(axis)
#endif
{
// Limit an axis. We have to differentiate: coasting, reversal of an axis, full stop.
float v_exit = previous_speed[axis] * smaller_speed_factor,
v_entry = current_speed[axis];
@ -2381,7 +2431,12 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
vmax_junction = safe_speed;
previous_safe_speed = safe_speed;
vmax_junction_sqr = sq(vmax_junction);
#if ENABLED(JUNCTION_DEVIATION)
vmax_junction_sqr = MIN(vmax_junction_sqr, sq(vmax_junction));
#else
vmax_junction_sqr = sq(vmax_junction);
#endif
#endif // Classic Jerk Limiting
@ -2466,7 +2521,12 @@ void Planner::buffer_sync_block() {
* extruder - target extruder
* millimeters - the length of the movement, if known
*/
bool Planner::buffer_segment(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder, const float &millimeters/*=0.0*/) {
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]
#endif
, const float &fr_mm_s, const uint8_t extruder, const float &millimeters/*=0.0*/
) {
// If we are cleaning, do not accept queuing of movements
if (cleaning_buffer_counter) return false;
@ -2534,6 +2594,9 @@ bool Planner::buffer_segment(const float &a, const float &b, const float &c, con
#if HAS_POSITION_FLOAT
, target_float
#endif
#if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION)
, delta_mm_cart
#endif
, fr_mm_s, extruder, millimeters
)
) return false;
@ -2543,24 +2606,84 @@ bool Planner::buffer_segment(const float &a, const float &b, const float &c, con
} // buffer_segment()
/**
* Directly set the planner XYZ position (and stepper positions)
* Add a new linear movement to the buffer.
* The target is cartesian, it's translated to delta/scara if
* needed.
*
*
* rx,ry,rz,e - target position in mm or degrees
* fr_mm_s - (target) speed of the move (mm/s)
* extruder - target extruder
* millimeters - the length of the movement, if known
* inv_duration - the reciprocal if the duration of the movement, if known (kinematic only if feeedrate scaling is enabled)
*/
bool Planner::buffer_line(const float &rx, const float &ry, const float &rz, const float &e, const float &fr_mm_s, const uint8_t extruder, const float millimeters
#if ENABLED(SCARA_FEEDRATE_SCALING)
, const float &inv_duration
#endif
) {
float raw[XYZE] = { rx, ry, rz, e };
#if HAS_POSITION_MODIFIERS
apply_modifiers(raw);
#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
};
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])) : fabs(delta_mm_cart[Z_AXIS]);
inverse_kinematics(raw);
#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,
feedrate = HYPOT(delta[A_AXIS] - position_float[A_AXIS], delta[B_AXIS] - position_float[B_AXIS]) * duration_recip;
#else
const float feedrate = fr_mm_s;
#endif
if (buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS]
#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;
return true;
}
else
return false;
#else
return buffer_segment(raw, fr_mm_s, extruder, millimeters);
#endif
} // buffer_line()
/**
* Directly set the planner ABC position (and stepper positions)
* converting mm (or angles for SCARA) into steps.
*
* On CORE machines stepper ABC will be translated from the given XYZ.
* The provided ABC position is in machine units.
*/
void Planner::_set_position_mm(const float &a, const float &b, const float &c, const float &e) {
void Planner::set_machine_position_mm(const float &a, const float &b, const float &c, const float &e) {
#if ENABLED(DISTINCT_E_FACTORS)
last_extruder = active_extruder;
#endif
position[A_AXIS] = LROUND(a * axis_steps_per_mm[A_AXIS]);
position[B_AXIS] = LROUND(b * axis_steps_per_mm[B_AXIS]);
position[C_AXIS] = LROUND(axis_steps_per_mm[C_AXIS] * (c +(
#if !IS_KINEMATIC && ENABLED(AUTO_BED_LEVELING_UBL)
leveling_active ? ubl.get_z_correction(a, b) :
#endif
0)
));
position[C_AXIS] = LROUND(c * axis_steps_per_mm[C_AXIS]);
position[E_AXIS] = LROUND(e * axis_steps_per_mm[_EINDEX]);
#if HAS_POSITION_FLOAT
position_float[A_AXIS] = a;
@ -2577,44 +2700,54 @@ void Planner::_set_position_mm(const float &a, const float &b, const float &c, c
stepper.set_position(position[A_AXIS], position[B_AXIS], position[C_AXIS], position[E_AXIS]);
}
void Planner::set_position_mm_kinematic(const float (&cart)[XYZE]) {
#if PLANNER_LEVELING
float raw[XYZ] = { cart[X_AXIS], cart[Y_AXIS], cart[Z_AXIS] };
apply_leveling(raw);
#else
const float (&raw)[XYZE] = cart;
void Planner::set_position_mm(const float &rx, const float &ry, const float &rz, const float &e) {
float raw[XYZE] = { rx, ry, rz, e };
#if HAS_POSITION_MODIFIERS
apply_modifiers(raw
#if HAS_LEVELING
, true
#endif
);
#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_position_mm(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], cart[E_AXIS]);
set_machine_position_mm(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS]);
#else
_set_position_mm(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], cart[E_AXIS]);
set_machine_position_mm(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], raw[E_AXIS]);
#endif
}
/**
* Setters for planner position (also setting stepper position).
*/
void Planner::set_position_mm(const AxisEnum axis, const float &v) {
void Planner::set_e_position_mm(const float &e) {
#if ENABLED(DISTINCT_E_FACTORS)
const uint8_t axis_index = axis + (axis == E_AXIS ? active_extruder : 0);
const uint8_t axis_index = E_AXIS + active_extruder;
last_extruder = active_extruder;
#else
const uint8_t axis_index = axis;
const uint8_t axis_index = E_AXIS;
#endif
position[axis] = LROUND(axis_steps_per_mm[axis_index] * (v + (
#if ENABLED(AUTO_BED_LEVELING_UBL)
axis == Z_AXIS && leveling_active ? ubl.get_z_correction(current_position[X_AXIS], current_position[Y_AXIS]) :
#endif
0)
));
#if ENABLED(FWRETRACT)
float e_new = e - fwretract.current_retract[active_extruder];
#else
const float e_new = e;
#endif
position[E_AXIS] = LROUND(axis_steps_per_mm[axis_index] * e_new);
#if HAS_POSITION_FLOAT
position_float[axis] = v;
position_float[E_AXIS] = e_new;
#endif
#if IS_KINEMATIC
position_cart[E_AXIS] = e;
#endif
if (has_blocks_queued())
buffer_sync_block();
else
stepper.set_position(axis, position[axis]);
stepper.set_position(E_AXIS, position[E_AXIS]);
}
// Recalculate the steps/s^2 acceleration rates, based on the mm/s^2
@ -2638,7 +2771,7 @@ void Planner::reset_acceleration_rates() {
// Recalculate position, steps_to_mm if axis_steps_per_mm changes!
void Planner::refresh_positioning() {
LOOP_XYZE_N(i) steps_to_mm[i] = 1.0f / axis_steps_per_mm[i];
set_position_mm_kinematic(current_position);
set_position_mm(current_position);
reset_acceleration_rates();
}

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

@ -45,6 +45,10 @@
#include "../libs/vector_3.h"
#endif
#if ENABLED(FWRETRACT)
#include "../feature/fwretract.h"
#endif
enum BlockFlagBit : char {
// Recalculate trapezoids on entry junction. For optimization.
BLOCK_BIT_RECALCULATE,
@ -151,7 +155,7 @@ typedef struct {
} block_t;
#define HAS_POSITION_FLOAT (ENABLED(LIN_ADVANCE) || HAS_FEEDRATE_SCALING)
#define HAS_POSITION_FLOAT (ENABLED(LIN_ADVANCE) || ENABLED(SCARA_FEEDRATE_SCALING))
#define BLOCK_MOD(n) ((n)&(BLOCK_BUFFER_SIZE-1))
@ -210,14 +214,22 @@ class Planner {
#if ENABLED(JUNCTION_DEVIATION)
static float junction_deviation_mm; // (mm) M205 J
#if ENABLED(LIN_ADVANCE)
#if ENABLED(DISTINCT_E_FACTORS)
static float max_e_jerk[EXTRUDERS]; // Calculated from junction_deviation_mm
#else
static float max_e_jerk;
#endif
static float max_e_jerk // Calculated from junction_deviation_mm
#if ENABLED(DISTINCT_E_FACTORS)
[EXTRUDERS]
#endif
;
#endif
#else
static float max_jerk[XYZE]; // (mm/s^2) M205 XYZE - The largest speed change requiring no acceleration.
#endif
#if HAS_CLASSIC_JERK
static float max_jerk[
#if ENABLED(JUNCTION_DEVIATION) && ENABLED(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
];
#endif
#if HAS_LEVELING
@ -240,6 +252,10 @@ class Planner {
static float position_float[XYZE];
#endif
#if IS_KINEMATIC
static float position_cart[XYZE];
#endif
#if ENABLED(SKEW_CORRECTION)
#if ENABLED(SKEW_CORRECTION_GCODE)
static float xy_skew_factor;
@ -410,6 +426,8 @@ 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 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)) {
@ -420,29 +438,76 @@ 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]); }
#endif // SKEW_CORRECTION
#if PLANNER_LEVELING || HAS_UBL_AND_CURVES
#if HAS_LEVELING
/**
* 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]);
#endif
#if PLANNER_LEVELING
#define ARG_X float rx
#define ARG_Y float ry
#define ARG_Z float rz
static void unapply_leveling(float raw[XYZ]);
#else
#define ARG_X const float &rx
#define ARG_Y const float &ry
#define ARG_Z const float &rz
#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]); }
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]); }
#endif
#if HAS_POSITION_MODIFIERS
FORCE_INLINE static void apply_modifiers(float (&pos)[XYZE]
#if HAS_LEVELING
, bool leveling =
#if PLANNER_LEVELING
true
#else
false
#endif
#endif
) {
#if ENABLED(SKEW_CORRECTION)
skew(pos);
#endif
#if HAS_LEVELING
if (leveling)
apply_leveling(pos);
#endif
#if ENABLED(FWRETRACT)
apply_retract(pos);
#endif
}
FORCE_INLINE static void unapply_modifiers(float (&pos)[XYZE]
#if HAS_LEVELING
, bool leveling =
#if PLANNER_LEVELING
true
#else
false
#endif
#endif
) {
#if ENABLED(FWRETRACT)
unapply_retract(pos);
#endif
#if HAS_LEVELING
if (leveling)
unapply_leveling(pos);
#endif
#if ENABLED(SKEW_CORRECTION)
unskew(pos);
#endif
}
#endif // HAS_POSITION_MODIFIERS
// Number of moves currently in the planner including the busy block, if any
FORCE_INLINE static uint8_t movesplanned() { return BLOCK_MOD(block_buffer_head - block_buffer_tail); }
@ -489,7 +554,10 @@ class Planner {
*/
static bool _buffer_steps(const int32_t (&target)[XYZE]
#if HAS_POSITION_FLOAT
, const float (&target_float)[XYZE]
, const float (&target_float)[ABCE]
#endif
#if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION)
, const float (&delta_mm_cart)[XYZE]
#endif
, float fr_mm_s, const uint8_t extruder, const float &millimeters=0.0
);
@ -511,6 +579,9 @@ class Planner {
#if HAS_POSITION_FLOAT
, const float (&target_float)[XYZE]
#endif
#if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION)
, const float (&delta_mm_cart)[XYZE]
#endif
, float fr_mm_s, const uint8_t extruder, const float &millimeters=0.0
);
@ -520,6 +591,13 @@ class Planner {
*/
static void buffer_sync_block();
#if IS_KINEMATIC
private:
// Allow do_homing_move to access internal functions, such as buffer_segment.
friend void do_homing_move(const AxisEnum, const float, const float);
#endif
/**
* Planner::buffer_segment
*
@ -532,74 +610,83 @@ class Planner {
* extruder - target extruder
* millimeters - the length of the movement, if known
*/
static bool buffer_segment(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder, const float &millimeters=0.0);
static void _set_position_mm(const float &a, const float &b, const float &c, const float &e);
/**
* Add a new linear movement to the buffer.
* The target is NOT translated to delta/scara
*
* Leveling will be applied to input on cartesians.
* Kinematic machines should call buffer_line_kinematic (for leveled moves).
* (Cartesians may also call buffer_line_kinematic.)
*
* rx,ry,rz,e - target position in mm or degrees
* fr_mm_s - (target) speed of the move (mm/s)
* extruder - target extruder
* millimeters - the length of the movement, if known
*/
FORCE_INLINE static bool buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, const float &fr_mm_s, const uint8_t extruder, const float millimeters = 0.0) {
#if PLANNER_LEVELING && IS_CARTESIAN
apply_leveling(rx, ry, rz);
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]
#endif
return buffer_segment(rx, ry, rz, e, fr_mm_s, extruder, millimeters);
, const float &fr_mm_s, const uint8_t extruder, const float &millimeters=0.0
);
FORCE_INLINE static bool buffer_segment(const float (&abce)[ABCE]
#if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION)
, const float (&delta_mm_cart)[XYZE]
#endif
, const float &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]
#if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION)
, delta_mm_cart
#endif
, fr_mm_s, extruder, millimeters);
}
public:
/**
* Add a new linear movement to the buffer.
* The target is cartesian, it's translated to delta/scara if
* needed.
*
* cart - x,y,z,e CARTESIAN target in mm
*
* rx,ry,rz,e - target position in mm or degrees
* fr_mm_s - (target) speed of the move (mm/s)
* extruder - target extruder
* millimeters - the length of the movement, if known
* inv_duration - the reciprocal if the duration of the movement, if known (kinematic only if feeedrate scaling is enabled)
*/
FORCE_INLINE static bool buffer_line_kinematic(const float (&cart)[XYZE], const float &fr_mm_s, const uint8_t extruder, const float millimeters = 0.0) {
#if PLANNER_LEVELING
float raw[XYZ] = { cart[X_AXIS], cart[Y_AXIS], cart[Z_AXIS] };
apply_leveling(raw);
#else
const float (&raw)[XYZE] = cart;
static bool buffer_line(const float &rx, const float &ry, const float &rz, const float &e, const float &fr_mm_s, const uint8_t extruder, const float millimeters=0.0
#if ENABLED(SCARA_FEEDRATE_SCALING)
, const float &inv_duration=0.0
#endif
#if IS_KINEMATIC
inverse_kinematics(raw);
return buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], cart[E_AXIS], fr_mm_s, extruder, millimeters);
#else
return buffer_segment(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], cart[E_AXIS], fr_mm_s, extruder, millimeters);
);
FORCE_INLINE static bool buffer_line(const float (&cart)[XYZE], const float &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
#if ENABLED(SCARA_FEEDRATE_SCALING)
, inv_duration
#endif
);
}
/**
* Set the planner.position and individual stepper positions.
* Used by G92, G28, G29, and other procedures.
*
* The supplied position is in the cartesian coordinate space and is
* translated in to machine space as needed. Modifiers such as leveling
* and skew are also applied.
*
* Multiplies by axis_steps_per_mm[] and does necessary conversion
* for COREXY / COREXZ / COREYZ to set the corresponding stepper positions.
*
* Clears previous speed values.
*/
FORCE_INLINE static void set_position_mm(ARG_X, ARG_Y, ARG_Z, const float &e) {
#if PLANNER_LEVELING && IS_CARTESIAN
apply_leveling(rx, ry, rz);
#endif
_set_position_mm(rx, ry, rz, e);
}
static void set_position_mm_kinematic(const float (&cart)[XYZE]);
static void set_position_mm(const AxisEnum axis, const float &v);
FORCE_INLINE static void set_z_position_mm(const float &z) { set_position_mm(Z_AXIS, z); }
FORCE_INLINE static void set_e_position_mm(const float &e) { set_position_mm(E_AXIS, e); }
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]); }
static void set_e_position_mm(const float &e);
/**
* Set the planner.position and individual stepper positions.
*
* The supplied position is in machine space, and no additional
* 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]); }
/**
* Get an axis position according to stepper position(s)
@ -756,16 +843,14 @@ class Planner {
static void autotemp_M104_M109();
#endif
#if ENABLED(JUNCTION_DEVIATION)
#if ENABLED(JUNCTION_DEVIATION) && ENABLED(LIN_ADVANCE)
FORCE_INLINE static void recalculate_max_e_jerk() {
#define GET_MAX_E_JERK(N) SQRT(SQRT(0.5) * junction_deviation_mm * (N) * RECIPROCAL(1.0 - SQRT(0.5)))
#if ENABLED(LIN_ADVANCE)
#if ENABLED(DISTINCT_E_FACTORS)
for (uint8_t i = 0; i < EXTRUDERS; i++)
max_e_jerk[i] = GET_MAX_E_JERK(max_acceleration_mm_per_s2[E_AXIS + i]);
#else
max_e_jerk = GET_MAX_E_JERK(max_acceleration_mm_per_s2[E_AXIS]);
#endif
#if ENABLED(DISTINCT_E_FACTORS)
for (uint8_t i = 0; i < EXTRUDERS; i++)
max_e_jerk[i] = GET_MAX_E_JERK(max_acceleration_mm_per_s2[E_AXIS + i]);
#else
max_e_jerk = GET_MAX_E_JERK(max_acceleration_mm_per_s2[E_AXIS]);
#endif
}
#endif

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

@ -190,15 +190,15 @@ void cubic_b_spline(const float position[NUM_AXIS], const float target[NUM_AXIS]
bez_target[E_AXIS] = interp(position[E_AXIS], target[E_AXIS], t);
clamp_to_software_endstops(bez_target);
#if HAS_UBL_AND_CURVES
float pos[XYZ] = { bez_target[X_AXIS], bez_target[Y_AXIS], bez_target[Z_AXIS] };
#if HAS_LEVELING && !PLANNER_LEVELING
float pos[XYZE] = { bez_target[X_AXIS], bez_target[Y_AXIS], bez_target[Z_AXIS], bez_target[E_AXIS] };
planner.apply_leveling(pos);
if (!planner.buffer_segment(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], bez_target[E_AXIS], fr_mm_s, active_extruder))
break;
#else
if (!planner.buffer_line_kinematic(bez_target, fr_mm_s, extruder))
break;
const float (&pos)[XYZE] = bez_target;
#endif
if (!planner.buffer_line(pos, fr_mm_s, active_extruder, step))
break;
}
}

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

@ -537,7 +537,7 @@ static bool do_probe_move(const float z, const float fr_mm_s) {
set_current_from_steppers_for_axis(Z_AXIS);
// Tell the planner where we actually are
SYNC_PLAN_POSITION_KINEMATIC();
sync_plan_position();
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("<<< do_probe_move", current_position);

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

@ -104,7 +104,7 @@ void forward_kinematics_SCARA(const float &a, const float &b) {
* 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 float (&raw)[XYZ]) {
static float C2, S2, SK1, SK2, THETA, PSI;

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

@ -24,8 +24,7 @@
* scara.h - SCARA-specific functions
*/
#ifndef __SCARA_H__
#define __SCARA_H__
#pragma once
#include "../core/macros.h"
@ -38,9 +37,11 @@ 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]);
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 forward_kinematics_SCARA(const float &a, const float &b);
void scara_report_positions();
#endif // __SCARA_H__

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

@ -134,7 +134,7 @@
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("(1) Raise Z-Axis", current_position);
#endif
planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[Z_AXIS], active_extruder);
planner.buffer_line(current_position, planner.max_feedrate_mm_s[Z_AXIS], active_extruder);
planner.synchronize();
// STEP 2
@ -145,7 +145,7 @@
DEBUG_POS("Moving ParkPos", current_position);
}
#endif
planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[X_AXIS], active_extruder);
planner.buffer_line(current_position, planner.max_feedrate_mm_s[X_AXIS], active_extruder);
planner.synchronize();
// STEP 3
@ -163,7 +163,7 @@
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("Move away from parked extruder", current_position);
#endif
planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[X_AXIS], active_extruder);
planner.buffer_line(current_position, planner.max_feedrate_mm_s[X_AXIS], active_extruder);
planner.synchronize();
// STEP 5
@ -178,12 +178,12 @@
// STEP 6
current_position[X_AXIS] = grabpos + (tmp_extruder ? -10 : 10);
planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[X_AXIS], active_extruder);
planner.buffer_line(current_position, planner.max_feedrate_mm_s[X_AXIS], active_extruder);
current_position[X_AXIS] = grabpos;
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("(6) Unpark extruder", current_position);
#endif
planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[X_AXIS]/2, active_extruder);
planner.buffer_line(current_position, planner.max_feedrate_mm_s[X_AXIS]/2, active_extruder);
planner.synchronize();
// Step 7
@ -191,7 +191,7 @@
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("(7) Move midway between hotends", current_position);
#endif
planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[X_AXIS], active_extruder);
planner.buffer_line(current_position, planner.max_feedrate_mm_s[X_AXIS], active_extruder);
planner.synchronize();
#if ENABLED(DEBUG_LEVELING_FEATURE)
SERIAL_ECHOLNPGM("Autopark done.");
@ -241,7 +241,7 @@
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("(1) Raise Z-Axis", current_position);
#endif
planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[Z_AXIS], active_extruder);
planner.buffer_line(current_position, planner.max_feedrate_mm_s[Z_AXIS], active_extruder);
planner.synchronize();
// STEP 2
@ -252,14 +252,14 @@
DEBUG_POS("Move X SwitchPos", current_position);
}
#endif
planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[X_AXIS], active_extruder);
planner.buffer_line(current_position, planner.max_feedrate_mm_s[X_AXIS], active_extruder);
planner.synchronize();
current_position[Y_AXIS] = SWITCHING_TOOLHEAD_Y_POS - SWITCHING_TOOLHEAD_Y_SECURITY;
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("Move Y SwitchPos + Security", current_position);
#endif
planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[Y_AXIS], active_extruder);
planner.buffer_line(current_position, planner.max_feedrate_mm_s[Y_AXIS], active_extruder);
planner.synchronize();
// STEP 3
@ -273,14 +273,14 @@
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("Move Y SwitchPos", current_position);
#endif
planner.buffer_line_kinematic(current_position,(planner.max_feedrate_mm_s[Y_AXIS] * 0.5), active_extruder);
planner.buffer_line(current_position,(planner.max_feedrate_mm_s[Y_AXIS] * 0.5), active_extruder);
planner.synchronize();
safe_delay(200);
current_position[Y_AXIS] -= SWITCHING_TOOLHEAD_Y_CLEAR;
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("Move back Y clear", current_position);
#endif
planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[Y_AXIS], active_extruder); // move away from docked toolhead
planner.buffer_line(current_position, planner.max_feedrate_mm_s[Y_AXIS], active_extruder); // move away from docked toolhead
planner.synchronize();
// STEP 4
@ -291,13 +291,13 @@
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("Move to new toolhead X", current_position);
#endif
planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[X_AXIS], active_extruder);
planner.buffer_line(current_position, planner.max_feedrate_mm_s[X_AXIS], active_extruder);
planner.synchronize();
current_position[Y_AXIS] = SWITCHING_TOOLHEAD_Y_POS - SWITCHING_TOOLHEAD_Y_SECURITY;
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("Move Y SwitchPos + Security", current_position);
#endif
planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[Y_AXIS], active_extruder);
planner.buffer_line(current_position, planner.max_feedrate_mm_s[Y_AXIS], active_extruder);
planner.synchronize();
// STEP 5
@ -308,7 +308,7 @@
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("Move Y SwitchPos", current_position);
#endif
planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[Y_AXIS] * 0.5, active_extruder);
planner.buffer_line(current_position, planner.max_feedrate_mm_s[Y_AXIS] * 0.5, active_extruder);
planner.synchronize();
safe_delay(200);
@ -319,7 +319,7 @@
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("Move back Y clear", current_position);
#endif
planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[Y_AXIS], active_extruder); // move away from docked toolhead
planner.buffer_line(current_position, planner.max_feedrate_mm_s[Y_AXIS], active_extruder); // move away from docked toolhead
planner.synchronize();
// STEP 6
@ -524,7 +524,7 @@ void tool_change(const uint8_t tmp_extruder, const float fr_mm_s/*=0.0*/, bool n
#if ENABLED(SWITCHING_NOZZLE)
// Always raise by at least 1 to avoid workpiece
current_position[Z_AXIS] += MAX(-zdiff, 0.0) + 1;
planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[Z_AXIS], active_extruder);
planner.buffer_line(current_position, planner.max_feedrate_mm_s[Z_AXIS], active_extruder);
move_nozzle_servo(tmp_extruder);
#endif
#endif
@ -549,7 +549,7 @@ void tool_change(const uint8_t tmp_extruder, const float fr_mm_s/*=0.0*/, bool n
#endif // !DUAL_X_CARRIAGE
// Tell the planner the new "current position"
SYNC_PLAN_POSITION_KINEMATIC();
sync_plan_position();
#if ENABLED(DELTA)
//LOOP_XYZ(i) update_software_endstops(i); // or modify the constrain function
@ -563,7 +563,7 @@ void tool_change(const uint8_t tmp_extruder, const float fr_mm_s/*=0.0*/, bool n
#if DISABLED(SWITCHING_NOZZLE)
// Do a small lift to avoid the workpiece in the move back (below)
current_position[Z_AXIS] += 1.0;
planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[Z_AXIS], active_extruder);
planner.buffer_line(current_position, planner.max_feedrate_mm_s[Z_AXIS], active_extruder);
#endif
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("Move back", destination);